From e94b670748461454efdd6f9db8155017632e2367 Mon Sep 17 00:00:00 2001 From: SuperJappie08 <36795178+SuperJappie08@users.noreply.github.com> Date: Thu, 3 Oct 2024 09:27:33 +0200 Subject: [PATCH 01/34] Remove Python cache files --- mirte_robot/__pycache__/linetrace.cpython-36.pyc | Bin 1926 -> 0 bytes mirte_robot/__pycache__/robot.cpython-36.pyc | Bin 7219 -> 0 bytes 2 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 mirte_robot/__pycache__/linetrace.cpython-36.pyc delete mode 100644 mirte_robot/__pycache__/robot.cpython-36.pyc diff --git a/mirte_robot/__pycache__/linetrace.cpython-36.pyc b/mirte_robot/__pycache__/linetrace.cpython-36.pyc deleted file mode 100644 index 9fc2b0843abab61a4ecb664b45e36360762084f2..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1926 zcmah}&2HQ_5Ed!w$6Bp-y>^fy4N|9ViYh?70eYwpY1$x#ds`GukN~=7aK@=$b%Fp`UHIv-in_33O#j(TK~Y8O5kuL4$1lEo0%`V-RRk$(T~4Gj`O#3 z?O~vQ14I1|f;*hlPTI~SCH9-9)Xr||!c1#6@lubpHh${EKerAhVHy%gxYY`0+&iOr zwA6jjJ3Qdwxs%3xg-0-VVQp1(b)-9E$~*6A-s3Uv!b+d7@*d1Mq=hRUX38eiiTl*Es?UM+F{>&&BcUpsQ~| zEZ~9zSH@_F$7;H8D))@w=AZ7qb1EjcwO6s&ffWO-d1S7~X&afZ95jq#51`cRmFH<{96W|%G~ zBK1@)#4K@*H&U{pg?dO8)1ZAx z#XvZ??T@pf;lgx^CL5Qvm?DS}qM?pUy`Ok;4NjDp4iYagv}hnEOT&c7TexCV&!P*g zho#;K!V>xd(!@q*f>H`p1Yd*O-d?Fvg+B|r?W#97IN0=6d?KeuNY}QDf+>%J5+(gkw z@c=~HKfdVeS=&t{#xRB6u4X4`gc+ULUMn%asrL%o$TjW|Rm9U%PwPCzoI^T(l(K&7;?1tp(W^yr+U}3o|mh8 z&F-iNP&S7M5h)Uf2sm&6fe=U>I3R=s2QFMVaNs}-BoK$l2QC~r@%`1^)idic!r9gS zRbBPh-(UT&`n&eoQYruG>&E+UKdvbMR0e)@^k2uH_hS&IFtw@JQLEajc-Cx9w7RW} z*02rHnzkw0jGYl}*3P2UnpP`k=TwrRH}kE6T@ZZ({i0nIeG~nXT@w9Fv%EpPndVsB zj&CRpr8aRsVYK7y2s?^) zf*oUzpsldu>`}Cn>@oH@+9`H|J%M(bEwL)v8Me%xL_5n)vZv6_x%1l#Y-Ldq?dqbk zsj*Y+X&_u=XV_`>9D0Y?^Xvui9%e7HFQ7fbUV`>s3C(vpH#>e^jWtq0o59~X{CVS0 zkFRX2fwHabt1uCIj?OetgXy3qGe9%U1kL)^wzaRImlGU$zp!0o8NWno`( zGKP_Hmfu(T&(WKpUSDmHjLPmafqGu~%5q;5s|tY{X!8my&MUJDE6pnYWFmLF5@=_X zJ70fS0p}E%f{lNysq_SM?5m%sJR_u|-`9oo>D`q;hx8MDLrAX#dSE2d{TV5FIxryl zuR`)Tn-sF%6SC0nn?ly??&-jUtdo6uOOX`Hs^y}iq$_VB_1I}pJ;cM zV`&RQ+K+@Z^!s@s?HHux2c#_q`2lH6xd(?CMQ^E=&* zs%q!EF87d)+_oQ@o?qjBudv$dxSOl3j^E+EeDvVCz7rblTFbRFj@NZvW{(k<)7|rL zb=pp?+jtYwaU3Ytv(eoArC1Xw^z0-uejMHej)Y8mA2|rrG3Rt7kvKh<-|aZH_Eyuau7sJI@9sAI(BkeEX_@mP)Nkw8-o0X* zm)5Rb->@@RuWyL<+V#tquV1q>XV=bNzIrh<{6@3&{i~NRoWDfo z4lAimlht7!yNe&}aY7V7I8BJOi;#_jGx835a`5SV=YDRckX?ILu;vvss+Tg+uXD>!gO9}iWaAK zYzatgdmZjUqUhKrS;h+u5r7kemJob)F;-Ee__0*0(R5iVQat}u;mdHr|D0ak@%U=9 z15aJ`cs+`ZfovfL;>@wi=~Q2VFI$cyT*_I53~v)e(TZ9|jeqsLnlUSC#mKAZ;cwY2 zmx^)!gqHM9>Y|siFf*ebR~OV%Ml!b)&0%KAfF%0W$JJh06c#JV>t%&ODU*_=s4&Xt zpu$k7ZrE1mD8H&}RgG6L&L=^tDnCJe(m+I>L@zADg*Fh#2))O} zzL{9$n@{2a?^OD#uWqZNZczf2skb$LhziSiRA3T$Ut1F?AK<32R3JoOfD;1;5KhGq z)*DFEmyu!4A;EkTE4@63B6Mx3hSpmcLOjG?7n=e%9XJv>>4uO8>OAz;*8**u3SptQ zIn0FytNFF69${O;2ufJE>OmbAfG|lWAA!p=Koqs8_9i5xWF8q8q0-r^6B7C%Ug`@g zK7&qxvKhLY8bGFFOvBj8XpHHzia@LfxkVdwrlHAzBF)@(N{F=YxRZXF@P)G6%4dUR> zLfUAU5QL0-1x%lUCx%&8doj#(`r(yzm|iN37%9unL|JVl5IP^Dlj8d+4mjeNR97@R z1Vg%C8W2p~*JMB-V9CAUP)816;V;s_D8o$v$XVg($w~BsAwaVd&@h*b3j#I5jQDcW zV8$d?Q)ovC$qOJ!ygnEjtTFu3AEwY?vl!ZaToM|=lF%r|l2Bzxfq+N4qtMilOtdw` zo4e}+)jT*u9qA*28HZ#Qqx@AOqtsXc3aUjmAmS%rial#erj~*EA$-!NGgy`41G)$s zB|2a;MNJFK61e+uoqZ549K30b^s^LLnjr=mJ`XaK>cHkVI_(FdJNF=T5rYOoHN=NFd z7MH&n0x>B;q#V+R2#buqG{4`U7-IYU|B0ajNCy=EyA&5PLu{Y>*`%Tfm<&Bi4Rq^+ zv_N48RRt<6MX5oL!jH0~bGO0$omvxLKFtotNj;5aoWhoWiO9hDjeD^Mck2`s_SDca z2ZHG9ZIqqIFn1$*$2oe%?;+Krt0^i`EVVZ+y>5_87JIZExg{!#2DSvJiwmcCLsSYm zH*1_eb%(Zc84HI_v2x7yzmU`E|3)P24_GfjXXL?}GDs5)g-BuoQoKsRqHXI@UzDQo z+DPAa7ByL;z1gwLl&zia&P{yF-Ez5|jk}0+QKE`eeV|GypepP(YkQQX|42Z|4HVSz zNdY)8KjgZ_5(T+Qg?E%*zto7aCwhcllF9R*0M;@*I)piM&MQWg=&Yyh7x4B5x2mPvioTH;H_a$R#3g5xGKSjR@(O zzYPLw*Zf3bK`8BclpieBx@qZ_Wf}RbWjx^5w9IH` z!N?m{Mo6e*vHX$NqS*8Ctz1K)o75WgzR;lSlTmQOLY=#~WE4jD7g!_1;NFD4kEiNX zbVDJ|Ji1un$BBHI$XAI_v0;zle!|0LoU_?z;*|4joSU7N>+?OQ Date: Thu, 3 Oct 2024 14:45:46 +0200 Subject: [PATCH 02/34] Initial Updata for ROS 2 --- .gitignore | 4 + mirte_robot/robot.py | 426 ++++++++++++++++++++++++++++++++----------- setup.py | 4 +- 3 files changed, 326 insertions(+), 108 deletions(-) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..70df8a3 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +**/__pycache__ + +COLCON_IGNORE +.colcon_root diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index 5ff597c..52f3b65 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -1,41 +1,93 @@ #!/usr/bin/env python import time -import rospy -import rosservice -import signal -import sys -import math -import atexit - -# TODO: check if we need the telemetrix version of this? -#sys.path.append('/usr/local/lib/python2.7/dist-packages/PyMata-2.20-py2.7.egg') # Needed for jupyter notebooks -#sys.path.append('/usr/local/lib/python2.7/dist-packages/pyserial-3.4-py2.7.egg') - -import message_filters -from geometry_msgs.msg import Twist -from std_msgs.msg import Int32 -from std_msgs.msg import String -from std_msgs.msg import Empty -from mirte_msgs.msg import * - -from mirte_msgs.srv import * -from std_srvs.srv import * +from typing import TYPE_CHECKING, Optional +import os + +import platform + +import rclpy +import rclpy.node +from rclpy.validate_namespace import validate_namespace + +if TYPE_CHECKING: + import rclpy.client + +from controller_manager_msgs.srv import SwitchController +from mirte_msgs.srv import ( + GetColor, + GetDistance, + GetEncoder, + GetIntensity, + GetIntensityDigital, + GetKeypad, + GetPinValue, + SetMotorSpeed, + SetOLEDImageLegacy, + SetPinValue, + SetServoAngle, +) +from rcl_interfaces.srv import ListParameters mirte = {} -class Robot(): - """ Robot API +# TODO: Make it equivalent to ROS 1 persistant +# SRV_QOSPROFILE = rclpy.qos.QoSProfile() - This class allows you to control the robot from Python. The getters and setters - are just wrappers calling ROS topics or services. +# FIXME: Maybe use encapsulation instead to prevent having lots of confusing and non relavent methods. +class Robot(rclpy.node.Node): + """Robot API + + This class allows you to control the robot from Python. The getters and setters + are just wrappers calling ROS topics or services. """ + # Implementation Notes: + # This class creates a hidden ROS Node for the communication. + # This should only be run once, however this can not be prevented from the web interface. + # There for the node is also anonymized with the current time. + + def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: str = "io"): + """Intialize the Mirte Robot API + + Parameters: + machine_namespace (Optional[str], optional): The Namespace from '/' to the ROS namespace for the specific Mirte. Defaults to "/{HOSTNAME}". (This only has to be changed when running the Robot API from a different machine directly. It is configured correctly for the Web interface) + hardware_namespace (str, optional): The namespace for the hardware peripherals. Defaults to "io". + """ + self._machine_namespace = ( + machine_namespace + if machine_namespace and validate_namespace(machine_namespace) + else "/" + platform.node().replace("-", "_").lower() + ) + self._hardware_namespace = ( + hardware_namespace + if validate_namespace( + hardware_namespace + if hardware_namespace.startswith("/") + else (self._machine_namespace + "/" + hardware_namespace) + ) + else "io" + ) + + ROS_DISTRO = os.getenv("ROS_DISTRO") + + rclpy.init() + + # This node should be only ran once. + # No 'anonymous' flag available, so use unix epoch nano seconds to pad name + super().__init__( + "_mirte_python_api_" + str(time.time_ns()), + namespace=self._machine_namespace, + start_parameter_services=True + ) + - def __init__(self): # Stop robot when exited - atexit.register(self.stop) + rclpy.get_default_context().on_shutdown(self._at_exit) - self.PWM = 3 #PrivateConstants.PWM when moving to Python3 + self.CONTROLLER = "diffbot_base_controller" + + # FIXME: Is this needed + self.PWM = 3 # PrivateConstants.PWM when moving to Python3 self.INPUT = 0 self.OUTPUT = 1 self.PULLUP = 11 @@ -45,28 +97,61 @@ def __init__(self): self.begin_time = time.time() self.last_call = 0 - # Call /stop and /start service to disable/enable the ROS diff_drive_controller - # By default this class will control the rbot though PWM (controller stopped). Only in case + # Call controller_manager/switch_controller service to disable/enable the ROS diff_drive_controller + # By default this class will control the robot though PWM (controller stopped). Only in case # the controller is needed, it will be enabled. - self.stop_controller_service = rospy.ServiceProxy('stop', Empty, persistent=True) - self.start_controller_service = rospy.ServiceProxy('start', Empty, persistent=True) + self.switch_controller_service = self.create_client( + SwitchController, "controller_manager/switch_controller" + ) # TODO: QOS? + + + if ROS_DISTRO[0] >= "i": # Chekc if the ROS Distro is IRON or newer + # Not available untill ROS Iron + if not self.wait_for_node(self._hardware_namespace + "telemetrix", 10): + self.get_logger().fatal(f"Telemetrix node at '{self.get_namespace() + self._hardware_namespace + 'telemetrix'}' was not found! Aborting") + exit(-1) + list_parameters: rclpy.client.Client = self.create_client( + ListParameters, self._hardware_namespace + "/telemetrix/list_parameters" + ) # Service for motor speed - self.motors = {} - if rospy.has_param("/mirte/motor"): - self.motors = rospy.get_param("/mirte/motor") + self.motors = {} # FIXME: Is self.motors used? + # TODO: Does the `mirte` prefix belong here. + motors_future: rclpy.Future = list_parameters.call_async( + ListParameters.Request(prefixes=["motor"], depth=3) + ) + rclpy.spin_until_future_complete(self, motors_future) + + motor_prefixes: list[str] = motors_future.result().result.prefixes + if len(motor_prefixes) > 0: + self.motors = [ + motor_prefix.split(".")[-1] for motor_prefix in motor_prefixes + ] self.motor_services = {} for motor in self.motors: - self.motor_services[motor] = rospy.ServiceProxy('/mirte/set_' + self.motors[motor]["name"] + '_speed', SetMotorSpeed, persistent=True) + self.get_logger().info(f"Created service client for motor [{motor}]") + self.motor_services[motor] = self.create_client( + SetMotorSpeed, self._hardware_namespace + "/set_" + motor + "_speed" + ) # TODO: QOS - # Service for motor speed - if rospy.has_param("/mirte/servo"): - servos = rospy.get_param("/mirte/servo") - self.servo_services = {} - for servo in servos: - self.servo_services[servo] = rospy.ServiceProxy('/mirte/set_' + servos[servo]["name"] + '_servo_angle', SetServoAngle, persistent=True) + # Service for motor speed - rospy.init_node('mirte_python_api', anonymous=False) + # TODO: Does the `mirte` prefix belong here. + servo_future = list_parameters.call_async( + ListParameters.Request(prefixes=["servo"], depth=3) + ) + + rclpy.spin_until_future_complete(self, servo_future) + + servo_prefixes = servo_future.result().result.prefixes + if len(servo_prefixes) > 0: + servos = [servo_prefix.split(".")[-1] for servo_prefix in servo_prefixes] + self.servo_services: dict[str, rclpy.client.Client] = {} + for servo in servos: + self.get_logger().info(f"Created service client for servo [{servo}]") + self.servo_services[servo] = self.create_client( + SetServoAngle, self._hardware_namespace + "/set_" + servo + "_servo_angle" + ) # TODO: QOS FOR persistent=True ## Sensors ## The sensors are now just using a blocking service call. This is intentionally @@ -76,62 +161,154 @@ def __init__(self): ## maybe even to blockly. # Services for distance sensors - if rospy.has_param("/mirte/distance"): - distance_sensors = rospy.get_param("/mirte/distance") + distance_future = list_parameters.call_async( + ListParameters.Request(prefixes=["distance"], depth=3) + ) + + rclpy.spin_until_future_complete(self, distance_future) + + distance_prefixes = distance_future.result().result.prefixes + if len(distance_prefixes) > 0: + distance_sensors = [sensor.split(".")[-1] for sensor in distance_prefixes] self.distance_services = {} for sensor in distance_sensors: - self.distance_services[sensor] = rospy.ServiceProxy('/mirte/get_distance_' + distance_sensors[sensor]["name"], GetDistance, persistent=True) - - if rospy.has_param("/mirte/oled"): - oleds = rospy.get_param("/mirte/oled") + self.get_logger().info( + f"Created service client for distance sensor [{sensor}]" + ) + self.distance_services[sensor] = self.create_client( + GetDistance, self._hardware_namespace + "/get_distance_" + sensor + ) # TODO: QOS + + + # FIXME: Current parser does not play nice with this method + # Services for oled + oled_future = list_parameters.call_async( + # Depth needs to be 4 since it is a module. + ListParameters.Request(prefixes=["oled"], depth=4) + ) + + rclpy.spin_until_future_complete(self, oled_future) + + oled_prefixes = oled_future.result().result.prefixes + # FIXME: Not use legacy service + if len(oled_prefixes) > 0: + oleds = [oled.split(".")[-1] for oled in oled_prefixes] self.oled_services = {} for oled in oleds: - self.oled_services[oled] = rospy.ServiceProxy('/mirte/set_' + oleds[oled]["name"] + '_image', SetOLEDImage, persistent=True) + self.get_logger().info(f"Created service client for oled [{oled}]") + self.oled_services[oled] = self.create_client( + SetOLEDImageLegacy, self._hardware_namespace + "/set_" + oled + "_image_legacy" + ) # TODO: QOS # Services for intensity sensors (TODO: how to expose the digital version?) - if rospy.has_param("/mirte/intensity"): - intensity_sensors = rospy.get_param("/mirte/intensity") + intensity_future = list_parameters.call_async( + ListParameters.Request(prefixes=["intensity"], depth=3) + ) + + rclpy.spin_until_future_complete(self, intensity_future) + + intensity_prefixes = intensity_future.result().result.prefixes + if len(intensity_prefixes) > 0: + intensity_sensors = [sensor.split(".")[-1] for sensor in intensity_prefixes] self.intensity_services = {} # We can not get the types (analog and/or digital) of the intensity sensor # straight from the parameter server (it might be just set as the PCB without # explicit values. We can however deduct what is there by checking the # services. - service_list = rosservice.get_service_list() + service_list = set( + [ + service + for service, service_type in self.get_service_names_and_types() + ] + ) for sensor in intensity_sensors: - if "/mirte/get_intensity_" + intensity_sensors[sensor]["name"] in service_list: - self.intensity_services[sensor] = rospy.ServiceProxy('/mirte/get_intensity_' + intensity_sensors[sensor]["name"], GetIntensity, persistent=True) - if "/mirte/get_intensity_" + intensity_sensors[sensor]["name"] + "_digital" in service_list: - self.intensity_services[sensor + "_digital"] = rospy.ServiceProxy('/mirte/get_intensity_' + intensity_sensors[sensor]["name"] + "_digital", GetIntensityDigital, persistent=True) - + if self._hardware_namespace + "/get_intensity_" + sensor in service_list: + self.get_logger().info(f"Created service client for intensity [{sensor}]") + self.intensity_services[sensor] = self.create_client( + GetIntensity, self._hardware_namespace + "/get_intensity_" + sensor + ) # TODO: QOS + if self._hardware_namespace + "/get_intensity_" + sensor + "_digital" in service_list: + self.get_logger().info(f"Created service client for digital intensity [{sensor}]") + self.intensity_services[sensor + "_digital"] = self.create_client( + GetIntensityDigital, + self._hardware_namespace + "/get_intensity_" + sensor + "_digital", + ) # TODO: QOS # Services for encoder sensors - if rospy.has_param("/mirte/encoder"): - encoder_sensors = rospy.get_param("/mirte/encoder") + encoder_future = list_parameters.call_async( + ListParameters.Request(prefixes=["encoder"], depth=3) + ) + + rclpy.spin_until_future_complete(self, encoder_future) + + encoder_prefixes = encoder_future.result().result.prefixes + if len(encoder_prefixes) > 0: + encoder_sensors = [sensor.split(".")[-1] for sensor in encoder_prefixes] self.encoder_services = {} for sensor in encoder_sensors: - self.encoder_services[sensor] = rospy.ServiceProxy('/mirte/get_encoder_' + encoder_sensors[sensor]["name"], GetEncoder, persistent=True) + self.get_logger().info(f"Created service client for encoder [{sensor}]") + self.encoder_services[sensor] = self.create_client( + GetEncoder, + self._hardware_namespace + "/get_encoder_" + sensor, + ) # TODO: QOS for persistent=True, # Services for keypad sensors - if rospy.has_param("/mirte/keypad"): - keypad_sensors = rospy.get_param("/mirte/keypad") + keypad_future = list_parameters.call_async( + ListParameters.Request(prefixes=["keypad"], depth=3) + ) + + rclpy.spin_until_future_complete(self, keypad_future) + + keypad_prefixes = keypad_future.result().result.prefixes + if len(keypad_prefixes) > 0: + keypad_sensors = [sensor.split(".")[-1] for sensor in keypad_prefixes] self.keypad_services = {} for sensor in keypad_sensors: - self.keypad_services[sensor] = rospy.ServiceProxy('/mirte/get_keypad_' + keypad_sensors[sensor]["name"], GetKeypad, persistent=True) + self.get_logger().info(f"Created service client for keypad [{sensor}]") + self.keypad_services[sensor] = self.create_client( + GetKeypad, self._hardware_namespace + "/get_keypad_" + sensor + ) # TODO: Add QOS for persitent=true # Services for color sensors - if rospy.has_param("/mirte/color"): - color_sensors = rospy.get_param("/mirte/color") - self.color_services = {} - for sensor in color_sensors: - self.color_services[sensor] = rospy.ServiceProxy('/mirte/get_color_' + color_sensors[sensor]["name"], GetColor, persistent=True) + color_future = list_parameters.call_async( + ListParameters.Request(prefixes=["color"], depth=3) + ) - self.get_pin_value_service = rospy.ServiceProxy('/mirte/get_pin_value', GetPinValue, persistent=True) - self.set_pin_value_service = rospy.ServiceProxy('/mirte/set_pin_value', SetPinValue, persistent=True) + rclpy.spin_until_future_complete(self, color_future) - - signal.signal(signal.SIGINT, self._signal_handler) - signal.signal(signal.SIGTERM, self._signal_handler) + color_prefixes = color_future.result().result.prefixes + if len(color_prefixes) > 0: + color_sensors = [sensor.split(".")[-1] for sensor in color_prefixes] + self.color_services = {} + for sensor in color_sensors: + self.get_logger().info( + f"Created service client for color sensor [{sensor}]" + ) + self.color_services[sensor] = self.create_client( + GetColor, self._hardware_namespace + "/get_color_" + sensor + ) # TODO: Add QOS persitent + + self.get_pin_value_service = self.create_client( + GetPinValue, self._hardware_namespace + "/get_pin_value" + ) # TODO: Add QOS persitent + self.set_pin_value_service = self.create_client( + SetPinValue, self._hardware_namespace + "/set_pin_value" + ) # TODO: Add QoS persistent=True + + def _call_service(self, client: rclpy.client.Client, request: rclpy.client.SrvTypeRequest) -> rclpy.client.SrvTypeResponse: + client.wait_for_service() + + future_response = client.call_async(request) + rclpy.spin_until_future_complete(self, future_response) + return future_response.result() + + # FIXME: Check if services aer avaiable, if not don't hard error on: + # AttributeError: 'Robot' object has no attribute 'oled_services'. Did you mean: '_services'? + def _check_available(self, services: dict[str] | None, id: str) -> bool: + if services is None: + return False + return id in services def getTimestamp(self): """Gets the elapsed time in seconds since the initialization fo the Robot. @@ -152,9 +329,9 @@ def getTimeSinceLastCall(self): last_call = self.last_call self.last_call = time.time() if last_call == 0: - return 0 + return 0 else: - return time.time() - last_call + return time.time() - last_call def getDistance(self, sensor): """Gets data from a HC-SR04 distance sensor: calculated distance in meters. @@ -169,8 +346,8 @@ def getDistance(self, sensor): A maximum of 6 distance sensors is supported. """ - - dist = self.distance_services[sensor]() + dist = self._call_service(self.distance_services[sensor], GetDistance.Request()) + # FIXME: What to do about -inf, nan and inf? return dist.data def getIntensity(self, sensor, type="analog"): @@ -184,9 +361,11 @@ def getIntensity(self, sensor, type="analog"): int: Value of the sensor (0-255 when analog, 0-1 when digital). """ if type == "analog": - value = self.intensity_services[sensor]() + value = self._call_service(self.intensity_services[sensor], GetIntensity.Request()) if type == "digital": - value = self.intensity_services[sensor + "_digital"]() + value = self._call_service(self.intensity_services[sensor + "_digital"], + GetIntensityDigital.Request() + ) return value.data def getEncoder(self, sensor): @@ -199,7 +378,7 @@ def getEncoder(self, sensor): int: Number of encoder pulses since boot of the robot. """ - value = self.encoder_services[sensor]() + value = self._call_service(self.encoder_services[sensor], GetEncoder.Request()) return value.data def getKeypad(self, keypad): @@ -212,10 +391,10 @@ def getKeypad(self, keypad): str: The name of the button ('up', 'down', 'left', 'right', 'enter'). """ - - value = self.keypad_services[keypad]() + value = self._call_service(self.keypad_services[keypad], GetKeypad.Request()) return value.data + # TODO: Is this implemented? Message might be missing def getColor(self, sensor): """Gets the value of the color sensor. @@ -226,8 +405,13 @@ def getColor(self, sensor): {r, g, b, w}: Raw (0-65536) values per R(ed), G(reen), B(lue), and W(hite). """ - value = self.color_services[sensor]() - return {'r': value.color.color.r, 'g': value.color.color.g, 'b': value.color.color.b, 'w': value.color.color.w } + value = self._call_service(self.color_services[sensor], GetColor.Request())() + return { + "r": value.color.color.r, + "g": value.color.color.g, + "b": value.color.color.b, + "w": value.color.color.w, + } def getAnalogPinValue(self, pin): """Gets the input value of an analog pin. @@ -239,7 +423,9 @@ def getAnalogPinValue(self, pin): int: Value between 0-255. """ - value = self.get_pin_value_service(str(pin), "analog") + value = self._call_service(self.get_pin_value_service, + GetPinValue.Request(pin=str(pin), type="analog") + ) return value.data def setAnalogPinValue(self, pin, value): @@ -250,7 +436,9 @@ def setAnalogPinValue(self, pin, value): value (int): Value between 0-255. """ - value = self.set_pin_value_service(str(pin), "analog", value) + value = self._call_service(self.set_pin_value_service, + SetPinValue.Request(pin=str(pin), type="analog", value=value) + ) return value.status def setOLEDText(self, oled, text): @@ -260,7 +448,9 @@ def setOLEDText(self, oled, text): oled (str): The name of the sensor as defined in the configuration. text (str): String to be shown on the 128x64 OLED. """ - value = self.oled_services[oled]('text', str(text)) + value = self._call_service(self.oled_services[oled], + SetOLEDImageLegacy.Request(type="text", value=str(text)) + ) return value.status def setOLEDImage(self, oled, image): @@ -271,7 +461,9 @@ def setOLEDImage(self, oled, image): image (str): Image name as defined in the images folder of the mirte-oled-images repository (excl file extension). """ - value = self.oled_services[oled]('image', image) + value = self._call_service(self.oled_services[oled], + SetOLEDImageLegacy.Request(type="image", value=image) + ) return value.status def setOLEDAnimation(self, oled, animation): @@ -282,7 +474,9 @@ def setOLEDAnimation(self, oled, animation): animation (str): Animation (directory) name as defined in the animations folder of the mirte-oled-images repository. """ - value = self.oled_services[oled]('animation', animation) + value = self._call_service(self.oled_services[oled], + SetOLEDImageLegacy.Request(type="animation", value=animation) + ) return value.status def getDigitalPinValue(self, pin): @@ -295,7 +489,9 @@ def getDigitalPinValue(self, pin): bool: The input value. """ - value = self.get_pin_value_service(str(pin), "digital") + value = self._call_service(self.get_pin_value_service, + GetPinValue.Request(pin=str(pin), type="digital") + ) return value.data def setServoAngle(self, servo, angle): @@ -304,7 +500,7 @@ def setServoAngle(self, servo, angle): Parameters: servo (str): The name of the sensor as defined in the configuration. angle (int): The angle of the servo (range [0-360], but some servos - might be hysically limited to [0-180]. + might be physically limited to [0-180]. Returns: bool: True if set successfully. @@ -320,8 +516,7 @@ def setServoAngle(self, servo, angle): Warning: A maximum of 12 servos is supported. """ - - value = self.servo_services[servo](angle) + value = self._call_service(self.servo_services[servo], SetServoAngle.Request(angle=float(angle),degrees=SetServoAngle.Request.DEGREES)) return value.status def setDigitalPinValue(self, pin, value): @@ -331,7 +526,10 @@ def setDigitalPinValue(self, pin, value): pin (str): The pin number of an analog pin as printed on the microcontroller. value (bool): Value to set. """ - value = self.set_pin_value_service(str(pin), "digital", value) + + value = self._call_service(self.set_pin_value_service, + SetPinValue.Request(pin=str(pin), type="digital", value=value) + ) return value.status def setMotorSpeed(self, motor, value): @@ -339,14 +537,14 @@ def setMotorSpeed(self, motor, value): Parameters: motor (str): The name of the sensor as defined in the configuration. - value (int): The 'directional duty cycle' (range [-100, 100]) of the PWM + value (int): The 'directional duty cycle' (range [-100, 100]) of the PWM signal (-100: full backward, 0: stand still, 100: full forward). Returns: bool: True if set successfully. """ - motor = self.motor_services[motor](value) + motor = self._call_service(self.motor_services[motor], SetMotorSpeed.Request(speed=int(value))) return motor.status def setMotorControl(self, status): @@ -360,11 +558,10 @@ def setMotorControl(self, status): Returns: none """ - - if (status): - self.start_controller_service() + if status: + self._call_service(self.switch_controller_service, SwitchController.Request(activate_controllers=[self.CONTROLLER], activate_asap=True)) else: - self.stop_controller_service() + self._call_service(self.switch_controller_service, SwitchController.Request(deactivate_controllers=[self.CONTROLLER])) return def stop(self): @@ -375,23 +572,40 @@ def stop(self): or when it finished. """ - + print("STOPPED") for motor in self.motors: - self.setMotorSpeed(self.motors[motor]["name"], 0) + self.setMotorSpeed(motor, 0) - def _signal_handler(self, sig, frame): + def _at_exit(self): self.stop() - sys.exit() + rclpy.try_shutdown() + # We need a special function to initiate the Robot() because the main.py need to call the # init_node() (see: https://answers.ros.org/question/266612/rospy-init_node-inside-imported-file/) -def createRobot(): +def createRobot(machine_namespace: Optional[str] = None, hardware_namespace: str = "io"): """Creates and return instance of the robot class. + Parameters: + machine_namespace (Optional[str], optional): The Namespace from '/' to the ROS namespace for the specific Mirte. Defaults to "/{HOSTNAME}". (This only has to be changed when running the Robot API from a different machine directly. It is configured correctly for the Web interface) + hardware_namespace (str, optional): The namespace for the hardware peripherals. Defaults to "io". + Returns: Robot: The initialize Robot class. """ global mirte - mirte = Robot() + mirte = Robot(machine_namespace, hardware_namespace) return mirte + + +if __name__ == "__main__": + rob = createRobot() + print("1") + rob.setServoAngle("right", 0) + time.sleep(0.5) + print("next") + rob.setServoAngle("right", 90) + + rob.setOLEDText("right","laar") + diff --git a/setup.py b/setup.py index 9c932a8..a0974be 100644 --- a/setup.py +++ b/setup.py @@ -5,14 +5,14 @@ setuptools.setup( name="mirte_robot", - install_requires=["websocket_server", "rospkg"], + install_requires=["websocket_server", "rclpy"], # TODO: Require mirte_msgs? rcl_interfaces? controller_manager_msgs? version="0.1.0", author="Martin Klomp", author_email="m.klomp@tudelft.nl", description="Python API for the Mirte Robot", long_description=long_description, long_description_content_type="text/markdown", - url="https://github.com/zoef-robot/zoef_python", + url="https://github.com/mirte-robot/mirte_python", packages=['mirte_robot'], classifiers=[ "Programming Language :: Python :: 3", From bb9ce7fefc8684d6ad972f153481522e4639339a Mon Sep 17 00:00:00 2001 From: SuperJappie08 <36795178+SuperJappie08@users.noreply.github.com> Date: Thu, 3 Oct 2024 14:51:48 +0200 Subject: [PATCH 03/34] Update linetrace --- mirte_robot/linetrace.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mirte_robot/linetrace.py b/mirte_robot/linetrace.py index f7e4c66..e5b1a01 100644 --- a/mirte_robot/linetrace.py +++ b/mirte_robot/linetrace.py @@ -9,8 +9,8 @@ import multiprocessing from websocket_server import WebsocketServer -# Already load rospy (whicht takes long) and robot, so mirte.py does not need to do this anymore -import rospy +# Already load rclpy (whicht takes long) and robot, so mirte.py does not need to do this anymore +import rclpy from mirte_robot import robot # Global shared memory objects (TODO: check if we need shared memory, why is server working?) From e08e6e26b913a5e24f4583f091a0619e66c03402 Mon Sep 17 00:00:00 2001 From: mklomp Date: Wed, 26 Jun 2024 13:18:23 +0200 Subject: [PATCH 04/34] Create LICENSE --- LICENSE | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 LICENSE diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..fc45ba6 --- /dev/null +++ b/LICENSE @@ -0,0 +1,28 @@ +BSD 3-Clause License + +Copyright (c) 2024, Martin Klomp, MIRTE-team + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. From 3a1d1b328f180a89d419e6545569250eb0d43ca8 Mon Sep 17 00:00:00 2001 From: mklomp Date: Sat, 29 Jun 2024 19:27:39 +0200 Subject: [PATCH 05/34] Update setup.py to BSD license --- setup.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/setup.py b/setup.py index a0974be..8c6cd81 100644 --- a/setup.py +++ b/setup.py @@ -12,11 +12,11 @@ description="Python API for the Mirte Robot", long_description=long_description, long_description_content_type="text/markdown", - url="https://github.com/mirte-robot/mirte_python", + url="https://github.com/mirte-robot/mirte-python", packages=['mirte_robot'], classifiers=[ "Programming Language :: Python :: 3", - "License :: OSI Approved :: Apache Software License", + "License :: OSI Approved :: BSD License", "Operating System :: OS Independent", ], python_requires='>=3.6', From b117b14c1e354744b448fc5c93e2db8b95f32add Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Wed, 2 Oct 2024 19:04:33 +0000 Subject: [PATCH 06/34] Fixing linetrace running issue where play did not work all the time --- mirte_robot/linetrace.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mirte_robot/linetrace.py b/mirte_robot/linetrace.py index e5b1a01..2315fec 100644 --- a/mirte_robot/linetrace.py +++ b/mirte_robot/linetrace.py @@ -22,9 +22,9 @@ # 1) the code finishes # 2) the user stopped the process # 3) the websocket connection is closed -def stop_mirte(terminate = True): +def stop_mirte(): global running - if terminate: + if running.value: process.terminate() running.value = False @@ -68,8 +68,8 @@ def traceit(frame, event, arg): # Sending the linetrace 0 to the client server.send_message_to_all("0") - - stop_mirte(False) + running.value = False + stop_mirte() process = multiprocessing.Process(target = load_mirte_module, args=(stepper, do_step)) From 5d47382577b176d4750f13609b6230743dd01119 Mon Sep 17 00:00:00 2001 From: SuperJappie08 <36795178+SuperJappie08@users.noreply.github.com> Date: Thu, 3 Oct 2024 15:42:12 +0200 Subject: [PATCH 07/34] Clean up --- mirte_robot/robot.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index 52f3b65..c3e8a4e 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -80,7 +80,6 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: start_parameter_services=True ) - # Stop robot when exited rclpy.get_default_context().on_shutdown(self._at_exit) @@ -105,7 +104,7 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: ) # TODO: QOS? - if ROS_DISTRO[0] >= "i": # Chekc if the ROS Distro is IRON or newer + if ROS_DISTRO[0] >= "i": # Check if the ROS Distro is IRON or newer # Not available untill ROS Iron if not self.wait_for_node(self._hardware_namespace + "telemetrix", 10): self.get_logger().fatal(f"Telemetrix node at '{self.get_namespace() + self._hardware_namespace + 'telemetrix'}' was not found! Aborting") @@ -572,7 +571,6 @@ def stop(self): or when it finished. """ - print("STOPPED") for motor in self.motors: self.setMotorSpeed(motor, 0) From 67dec8dbae8a96b4a5ccfa22552f958a44ee209c Mon Sep 17 00:00:00 2001 From: SuperJappie08 <36795178+SuperJappie08@users.noreply.github.com> Date: Thu, 3 Oct 2024 17:07:46 +0200 Subject: [PATCH 08/34] Encapsulate Node instead of inheriting for cleaner API --- mirte_robot/robot.py | 74 ++++++++++++++++++++++---------------------- 1 file changed, 37 insertions(+), 37 deletions(-) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index c3e8a4e..abd43c4 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -34,7 +34,7 @@ # SRV_QOSPROFILE = rclpy.qos.QoSProfile() # FIXME: Maybe use encapsulation instead to prevent having lots of confusing and non relavent methods. -class Robot(rclpy.node.Node): +class Robot: """Robot API This class allows you to control the robot from Python. The getters and setters @@ -74,7 +74,7 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: # This node should be only ran once. # No 'anonymous' flag available, so use unix epoch nano seconds to pad name - super().__init__( + self._node = rclpy.node.Node( "_mirte_python_api_" + str(time.time_ns()), namespace=self._machine_namespace, start_parameter_services=True @@ -99,17 +99,17 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: # Call controller_manager/switch_controller service to disable/enable the ROS diff_drive_controller # By default this class will control the robot though PWM (controller stopped). Only in case # the controller is needed, it will be enabled. - self.switch_controller_service = self.create_client( + self.switch_controller_service = self._node.create_client( SwitchController, "controller_manager/switch_controller" ) # TODO: QOS? if ROS_DISTRO[0] >= "i": # Check if the ROS Distro is IRON or newer # Not available untill ROS Iron - if not self.wait_for_node(self._hardware_namespace + "telemetrix", 10): - self.get_logger().fatal(f"Telemetrix node at '{self.get_namespace() + self._hardware_namespace + 'telemetrix'}' was not found! Aborting") + if not self._node.wait_for_node(self._hardware_namespace + "telemetrix", 10): + self._node.get_logger().fatal(f"Telemetrix node at '{self._node.get_namespace() + self._hardware_namespace + 'telemetrix'}' was not found! Aborting") exit(-1) - list_parameters: rclpy.client.Client = self.create_client( + list_parameters: rclpy.client.Client = self._node.create_client( ListParameters, self._hardware_namespace + "/telemetrix/list_parameters" ) @@ -119,7 +119,7 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: motors_future: rclpy.Future = list_parameters.call_async( ListParameters.Request(prefixes=["motor"], depth=3) ) - rclpy.spin_until_future_complete(self, motors_future) + rclpy.spin_until_future_complete(self._node, motors_future) motor_prefixes: list[str] = motors_future.result().result.prefixes if len(motor_prefixes) > 0: @@ -128,8 +128,8 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: ] self.motor_services = {} for motor in self.motors: - self.get_logger().info(f"Created service client for motor [{motor}]") - self.motor_services[motor] = self.create_client( + self._node.get_logger().info(f"Created service client for motor [{motor}]") + self.motor_services[motor] = self._node.create_client( SetMotorSpeed, self._hardware_namespace + "/set_" + motor + "_speed" ) # TODO: QOS @@ -140,15 +140,15 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: ListParameters.Request(prefixes=["servo"], depth=3) ) - rclpy.spin_until_future_complete(self, servo_future) + rclpy.spin_until_future_complete(self._node, servo_future) servo_prefixes = servo_future.result().result.prefixes if len(servo_prefixes) > 0: servos = [servo_prefix.split(".")[-1] for servo_prefix in servo_prefixes] self.servo_services: dict[str, rclpy.client.Client] = {} for servo in servos: - self.get_logger().info(f"Created service client for servo [{servo}]") - self.servo_services[servo] = self.create_client( + self._node.get_logger().info(f"Created service client for servo [{servo}]") + self.servo_services[servo] = self._node.create_client( SetServoAngle, self._hardware_namespace + "/set_" + servo + "_servo_angle" ) # TODO: QOS FOR persistent=True @@ -164,17 +164,17 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: ListParameters.Request(prefixes=["distance"], depth=3) ) - rclpy.spin_until_future_complete(self, distance_future) + rclpy.spin_until_future_complete(self._node, distance_future) distance_prefixes = distance_future.result().result.prefixes if len(distance_prefixes) > 0: distance_sensors = [sensor.split(".")[-1] for sensor in distance_prefixes] self.distance_services = {} for sensor in distance_sensors: - self.get_logger().info( + self._node.get_logger().info( f"Created service client for distance sensor [{sensor}]" ) - self.distance_services[sensor] = self.create_client( + self.distance_services[sensor] = self._node.create_client( GetDistance, self._hardware_namespace + "/get_distance_" + sensor ) # TODO: QOS @@ -186,7 +186,7 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: ListParameters.Request(prefixes=["oled"], depth=4) ) - rclpy.spin_until_future_complete(self, oled_future) + rclpy.spin_until_future_complete(self._node, oled_future) oled_prefixes = oled_future.result().result.prefixes # FIXME: Not use legacy service @@ -194,8 +194,8 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: oleds = [oled.split(".")[-1] for oled in oled_prefixes] self.oled_services = {} for oled in oleds: - self.get_logger().info(f"Created service client for oled [{oled}]") - self.oled_services[oled] = self.create_client( + self._node.get_logger().info(f"Created service client for oled [{oled}]") + self.oled_services[oled] = self._node.create_client( SetOLEDImageLegacy, self._hardware_namespace + "/set_" + oled + "_image_legacy" ) # TODO: QOS @@ -204,7 +204,7 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: ListParameters.Request(prefixes=["intensity"], depth=3) ) - rclpy.spin_until_future_complete(self, intensity_future) + rclpy.spin_until_future_complete(self._node, intensity_future) intensity_prefixes = intensity_future.result().result.prefixes if len(intensity_prefixes) > 0: @@ -218,18 +218,18 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: service_list = set( [ service - for service, service_type in self.get_service_names_and_types() + for service, service_type in self._node.get_service_names_and_types() ] ) for sensor in intensity_sensors: if self._hardware_namespace + "/get_intensity_" + sensor in service_list: - self.get_logger().info(f"Created service client for intensity [{sensor}]") - self.intensity_services[sensor] = self.create_client( + self._node.get_logger().info(f"Created service client for intensity [{sensor}]") + self.intensity_services[sensor] = self._node.create_client( GetIntensity, self._hardware_namespace + "/get_intensity_" + sensor ) # TODO: QOS if self._hardware_namespace + "/get_intensity_" + sensor + "_digital" in service_list: - self.get_logger().info(f"Created service client for digital intensity [{sensor}]") - self.intensity_services[sensor + "_digital"] = self.create_client( + self._node.get_logger().info(f"Created service client for digital intensity [{sensor}]") + self.intensity_services[sensor + "_digital"] = self._node.create_client( GetIntensityDigital, self._hardware_namespace + "/get_intensity_" + sensor + "_digital", ) # TODO: QOS @@ -239,15 +239,15 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: ListParameters.Request(prefixes=["encoder"], depth=3) ) - rclpy.spin_until_future_complete(self, encoder_future) + rclpy.spin_until_future_complete(self._node, encoder_future) encoder_prefixes = encoder_future.result().result.prefixes if len(encoder_prefixes) > 0: encoder_sensors = [sensor.split(".")[-1] for sensor in encoder_prefixes] self.encoder_services = {} for sensor in encoder_sensors: - self.get_logger().info(f"Created service client for encoder [{sensor}]") - self.encoder_services[sensor] = self.create_client( + self._node.get_logger().info(f"Created service client for encoder [{sensor}]") + self.encoder_services[sensor] = self._node.create_client( GetEncoder, self._hardware_namespace + "/get_encoder_" + sensor, ) # TODO: QOS for persistent=True, @@ -257,15 +257,15 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: ListParameters.Request(prefixes=["keypad"], depth=3) ) - rclpy.spin_until_future_complete(self, keypad_future) + rclpy.spin_until_future_complete(self._node, keypad_future) keypad_prefixes = keypad_future.result().result.prefixes if len(keypad_prefixes) > 0: keypad_sensors = [sensor.split(".")[-1] for sensor in keypad_prefixes] self.keypad_services = {} for sensor in keypad_sensors: - self.get_logger().info(f"Created service client for keypad [{sensor}]") - self.keypad_services[sensor] = self.create_client( + self._node.get_logger().info(f"Created service client for keypad [{sensor}]") + self.keypad_services[sensor] = self._node.create_client( GetKeypad, self._hardware_namespace + "/get_keypad_" + sensor ) # TODO: Add QOS for persitent=true @@ -274,24 +274,24 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: ListParameters.Request(prefixes=["color"], depth=3) ) - rclpy.spin_until_future_complete(self, color_future) + rclpy.spin_until_future_complete(self._node, color_future) color_prefixes = color_future.result().result.prefixes if len(color_prefixes) > 0: color_sensors = [sensor.split(".")[-1] for sensor in color_prefixes] self.color_services = {} for sensor in color_sensors: - self.get_logger().info( + self._node.get_logger().info( f"Created service client for color sensor [{sensor}]" ) - self.color_services[sensor] = self.create_client( + self.color_services[sensor] = self._node.create_client( GetColor, self._hardware_namespace + "/get_color_" + sensor ) # TODO: Add QOS persitent - self.get_pin_value_service = self.create_client( + self.get_pin_value_service = self._node.create_client( GetPinValue, self._hardware_namespace + "/get_pin_value" ) # TODO: Add QOS persitent - self.set_pin_value_service = self.create_client( + self.set_pin_value_service = self._node.create_client( SetPinValue, self._hardware_namespace + "/set_pin_value" ) # TODO: Add QoS persistent=True @@ -299,7 +299,7 @@ def _call_service(self, client: rclpy.client.Client, request: rclpy.client.SrvTy client.wait_for_service() future_response = client.call_async(request) - rclpy.spin_until_future_complete(self, future_response) + rclpy.spin_until_future_complete(self._node, future_response) return future_response.result() # FIXME: Check if services aer avaiable, if not don't hard error on: @@ -605,5 +605,5 @@ def createRobot(machine_namespace: Optional[str] = None, hardware_namespace: str print("next") rob.setServoAngle("right", 90) - rob.setOLEDText("right","laar") + rob.setOLEDText("right", "laar") From f5bc23f07d86449b26eabd642a7715edbdd6679f Mon Sep 17 00:00:00 2001 From: SuperJappie08 <36795178+SuperJappie08@users.noreply.github.com> Date: Fri, 4 Oct 2024 17:18:51 +0200 Subject: [PATCH 09/34] Cleanup & oled fix --- mirte_robot/robot.py | 223 ++++++++++++++++++++++++++----------------- 1 file changed, 136 insertions(+), 87 deletions(-) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index abd43c4..5bedaf7 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -22,7 +22,8 @@ GetKeypad, GetPinValue, SetMotorSpeed, - SetOLEDImageLegacy, + SetOLEDFile, + SetOLEDText, SetPinValue, SetServoAngle, ) @@ -30,10 +31,9 @@ mirte = {} -# TODO: Make it equivalent to ROS 1 persistant -# SRV_QOSPROFILE = rclpy.qos.QoSProfile() +# No QoS Profiles are not set, but this might not be required, since they might already behave like ROS 1 persistant. + -# FIXME: Maybe use encapsulation instead to prevent having lots of confusing and non relavent methods. class Robot: """Robot API @@ -46,7 +46,9 @@ class Robot: # This should only be run once, however this can not be prevented from the web interface. # There for the node is also anonymized with the current time. - def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: str = "io"): + def __init__( + self, machine_namespace: Optional[str] = None, hardware_namespace: str = "io" + ): """Intialize the Mirte Robot API Parameters: @@ -75,9 +77,9 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: # This node should be only ran once. # No 'anonymous' flag available, so use unix epoch nano seconds to pad name self._node = rclpy.node.Node( - "_mirte_python_api_" + str(time.time_ns()), + "_mirte_python_api_" + str(time.time_ns()), namespace=self._machine_namespace, - start_parameter_services=True + start_parameter_services=True, ) # Stop robot when exited @@ -85,7 +87,6 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: self.CONTROLLER = "diffbot_base_controller" - # FIXME: Is this needed self.PWM = 3 # PrivateConstants.PWM when moving to Python3 self.INPUT = 0 self.OUTPUT = 1 @@ -101,13 +102,16 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: # the controller is needed, it will be enabled. self.switch_controller_service = self._node.create_client( SwitchController, "controller_manager/switch_controller" - ) # TODO: QOS? - + ) - if ROS_DISTRO[0] >= "i": # Check if the ROS Distro is IRON or newer - # Not available untill ROS Iron - if not self._node.wait_for_node(self._hardware_namespace + "telemetrix", 10): - self._node.get_logger().fatal(f"Telemetrix node at '{self._node.get_namespace() + self._hardware_namespace + 'telemetrix'}' was not found! Aborting") + if ROS_DISTRO[0] >= "i": # Check if the ROS Distro is IRON or newer + # Not available untill ROS Iron + if not self._node.wait_for_node( + self._hardware_namespace + "telemetrix", 10 + ): + self._node.get_logger().fatal( + f"Telemetrix node at '{self._node.get_namespace() + self._hardware_namespace + 'telemetrix'}' was not found! Aborting" + ) exit(-1) list_parameters: rclpy.client.Client = self._node.create_client( ListParameters, self._hardware_namespace + "/telemetrix/list_parameters" @@ -115,7 +119,6 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: # Service for motor speed self.motors = {} # FIXME: Is self.motors used? - # TODO: Does the `mirte` prefix belong here. motors_future: rclpy.Future = list_parameters.call_async( ListParameters.Request(prefixes=["motor"], depth=3) ) @@ -128,14 +131,15 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: ] self.motor_services = {} for motor in self.motors: - self._node.get_logger().info(f"Created service client for motor [{motor}]") + self._node.get_logger().info( + f"Created service client for motor [{motor}]" + ) self.motor_services[motor] = self._node.create_client( SetMotorSpeed, self._hardware_namespace + "/set_" + motor + "_speed" - ) # TODO: QOS + ) # Service for motor speed - # TODO: Does the `mirte` prefix belong here. servo_future = list_parameters.call_async( ListParameters.Request(prefixes=["servo"], depth=3) ) @@ -147,10 +151,13 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: servos = [servo_prefix.split(".")[-1] for servo_prefix in servo_prefixes] self.servo_services: dict[str, rclpy.client.Client] = {} for servo in servos: - self._node.get_logger().info(f"Created service client for servo [{servo}]") + self._node.get_logger().info( + f"Created service client for servo [{servo}]" + ) self.servo_services[servo] = self._node.create_client( - SetServoAngle, self._hardware_namespace + "/set_" + servo + "_servo_angle" - ) # TODO: QOS FOR persistent=True + SetServoAngle, + self._hardware_namespace + "/set_" + servo + "_servo_angle", + ) ## Sensors ## The sensors are now just using a blocking service call. This is intentionally @@ -176,28 +183,33 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: ) self.distance_services[sensor] = self._node.create_client( GetDistance, self._hardware_namespace + "/get_distance_" + sensor - ) # TODO: QOS - + ) - # FIXME: Current parser does not play nice with this method # Services for oled oled_future = list_parameters.call_async( - # Depth needs to be 4 since it is a module. - ListParameters.Request(prefixes=["oled"], depth=4) + ListParameters.Request(prefixes=["oled"], depth=3) ) rclpy.spin_until_future_complete(self._node, oled_future) oled_prefixes = oled_future.result().result.prefixes - # FIXME: Not use legacy service if len(oled_prefixes) > 0: oleds = [oled.split(".")[-1] for oled in oled_prefixes] self.oled_services = {} for oled in oleds: - self._node.get_logger().info(f"Created service client for oled [{oled}]") - self.oled_services[oled] = self._node.create_client( - SetOLEDImageLegacy, self._hardware_namespace + "/set_" + oled + "_image_legacy" - ) # TODO: QOS + self._node.get_logger().info( + f"Created service client for oled [{oled}]" + ) + self.oled_services[oled] = { + "text": self._node.create_client( + SetOLEDText, + self._hardware_namespace + "/oled/set_" + oled + "_text", + ), + "file": self._node.create_client( + SetOLEDFile, + self._hardware_namespace + "/oled/set_" + oled + "_file", + ), + } # Services for intensity sensors (TODO: how to expose the digital version?) intensity_future = list_parameters.call_async( @@ -222,17 +234,33 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: ] ) for sensor in intensity_sensors: - if self._hardware_namespace + "/get_intensity_" + sensor in service_list: - self._node.get_logger().info(f"Created service client for intensity [{sensor}]") + if ( + self._hardware_namespace + "/get_intensity_" + sensor + in service_list + ): + self._node.get_logger().info( + f"Created service client for intensity [{sensor}]" + ) self.intensity_services[sensor] = self._node.create_client( - GetIntensity, self._hardware_namespace + "/get_intensity_" + sensor - ) # TODO: QOS - if self._hardware_namespace + "/get_intensity_" + sensor + "_digital" in service_list: - self._node.get_logger().info(f"Created service client for digital intensity [{sensor}]") - self.intensity_services[sensor + "_digital"] = self._node.create_client( - GetIntensityDigital, - self._hardware_namespace + "/get_intensity_" + sensor + "_digital", - ) # TODO: QOS + GetIntensity, + self._hardware_namespace + "/get_intensity_" + sensor, + ) + if ( + self._hardware_namespace + "/get_intensity_" + sensor + "_digital" + in service_list + ): + self._node.get_logger().info( + f"Created service client for digital intensity [{sensor}]" + ) + self.intensity_services[sensor + "_digital"] = ( + self._node.create_client( + GetIntensityDigital, + self._hardware_namespace + + "/get_intensity_" + + sensor + + "_digital", + ) + ) # Services for encoder sensors encoder_future = list_parameters.call_async( @@ -246,11 +274,13 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: encoder_sensors = [sensor.split(".")[-1] for sensor in encoder_prefixes] self.encoder_services = {} for sensor in encoder_sensors: - self._node.get_logger().info(f"Created service client for encoder [{sensor}]") + self._node.get_logger().info( + f"Created service client for encoder [{sensor}]" + ) self.encoder_services[sensor] = self._node.create_client( GetEncoder, self._hardware_namespace + "/get_encoder_" + sensor, - ) # TODO: QOS for persistent=True, + ) # Services for keypad sensors keypad_future = list_parameters.call_async( @@ -264,10 +294,12 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: keypad_sensors = [sensor.split(".")[-1] for sensor in keypad_prefixes] self.keypad_services = {} for sensor in keypad_sensors: - self._node.get_logger().info(f"Created service client for keypad [{sensor}]") + self._node.get_logger().info( + f"Created service client for keypad [{sensor}]" + ) self.keypad_services[sensor] = self._node.create_client( GetKeypad, self._hardware_namespace + "/get_keypad_" + sensor - ) # TODO: Add QOS for persitent=true + ) # Services for color sensors color_future = list_parameters.call_async( @@ -276,6 +308,7 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: rclpy.spin_until_future_complete(self._node, color_future) + # FIXME: Add color sensor from v0.1.1 color_prefixes = color_future.result().result.prefixes if len(color_prefixes) > 0: color_sensors = [sensor.split(".")[-1] for sensor in color_prefixes] @@ -286,23 +319,25 @@ def __init__(self, machine_namespace: Optional[str] = None, hardware_namespace: ) self.color_services[sensor] = self._node.create_client( GetColor, self._hardware_namespace + "/get_color_" + sensor - ) # TODO: Add QOS persitent + ) self.get_pin_value_service = self._node.create_client( GetPinValue, self._hardware_namespace + "/get_pin_value" - ) # TODO: Add QOS persitent + ) self.set_pin_value_service = self._node.create_client( SetPinValue, self._hardware_namespace + "/set_pin_value" - ) # TODO: Add QoS persistent=True + ) - def _call_service(self, client: rclpy.client.Client, request: rclpy.client.SrvTypeRequest) -> rclpy.client.SrvTypeResponse: + def _call_service( + self, client: rclpy.client.Client, request: rclpy.client.SrvTypeRequest + ) -> rclpy.client.SrvTypeResponse: client.wait_for_service() - + future_response = client.call_async(request) rclpy.spin_until_future_complete(self._node, future_response) return future_response.result() - - # FIXME: Check if services aer avaiable, if not don't hard error on: + + # FIXME: Check if services are avaiable, if not don't hard error on: # AttributeError: 'Robot' object has no attribute 'oled_services'. Did you mean: '_services'? def _check_available(self, services: dict[str] | None, id: str) -> bool: if services is None: @@ -360,10 +395,13 @@ def getIntensity(self, sensor, type="analog"): int: Value of the sensor (0-255 when analog, 0-1 when digital). """ if type == "analog": - value = self._call_service(self.intensity_services[sensor], GetIntensity.Request()) + value = self._call_service( + self.intensity_services[sensor], GetIntensity.Request() + ) if type == "digital": - value = self._call_service(self.intensity_services[sensor + "_digital"], - GetIntensityDigital.Request() + value = self._call_service( + self.intensity_services[sensor + "_digital"], + GetIntensityDigital.Request(), ) return value.data @@ -422,8 +460,8 @@ def getAnalogPinValue(self, pin): int: Value between 0-255. """ - value = self._call_service(self.get_pin_value_service, - GetPinValue.Request(pin=str(pin), type="analog") + value = self._call_service( + self.get_pin_value_service, GetPinValue.Request(pin=str(pin), type="analog") ) return value.data @@ -435,8 +473,9 @@ def setAnalogPinValue(self, pin, value): value (int): Value between 0-255. """ - value = self._call_service(self.set_pin_value_service, - SetPinValue.Request(pin=str(pin), type="analog", value=value) + value = self._call_service( + self.set_pin_value_service, + SetPinValue.Request(pin=str(pin), type="analog", value=value), ) return value.status @@ -447,8 +486,8 @@ def setOLEDText(self, oled, text): oled (str): The name of the sensor as defined in the configuration. text (str): String to be shown on the 128x64 OLED. """ - value = self._call_service(self.oled_services[oled], - SetOLEDImageLegacy.Request(type="text", value=str(text)) + value = self._call_service( + self.oled_services[oled]["text"], SetOLEDText.Request(text=str(text)) ) return value.status @@ -457,11 +496,14 @@ def setOLEDImage(self, oled, image): Parameters: oled (str): The name of the sensor as defined in the configuration. - image (str): Image name as defined in the images folder of the mirte-oled-images repository (excl file extension). + image (str): Image name as defined in the images folder of the mirte-oled-images repository (optionally excl file extension). """ - value = self._call_service(self.oled_services[oled], - SetOLEDImageLegacy.Request(type="image", value=image) + value = self._call_service( + self.oled_services[oled]["file"], + SetOLEDFile.Request( + path=(image if ("." in image.split("/")[-1]) else (image + ".png")) + ), ) return value.status @@ -473,8 +515,8 @@ def setOLEDAnimation(self, oled, animation): animation (str): Animation (directory) name as defined in the animations folder of the mirte-oled-images repository. """ - value = self._call_service(self.oled_services[oled], - SetOLEDImageLegacy.Request(type="animation", value=animation) + value = self._call_service( + self.oled_services[oled]["file"], SetOLEDFile.Request(path=animation) ) return value.status @@ -488,8 +530,9 @@ def getDigitalPinValue(self, pin): bool: The input value. """ - value = self._call_service(self.get_pin_value_service, - GetPinValue.Request(pin=str(pin), type="digital") + value = self._call_service( + self.get_pin_value_service, + GetPinValue.Request(pin=str(pin), type="digital"), ) return value.data @@ -515,7 +558,12 @@ def setServoAngle(self, servo, angle): Warning: A maximum of 12 servos is supported. """ - value = self._call_service(self.servo_services[servo], SetServoAngle.Request(angle=float(angle),degrees=SetServoAngle.Request.DEGREES)) + value = self._call_service( + self.servo_services[servo], + SetServoAngle.Request( + angle=float(angle), degrees=SetServoAngle.Request.DEGREES + ), + ) return value.status def setDigitalPinValue(self, pin, value): @@ -526,8 +574,9 @@ def setDigitalPinValue(self, pin, value): value (bool): Value to set. """ - value = self._call_service(self.set_pin_value_service, - SetPinValue.Request(pin=str(pin), type="digital", value=value) + value = self._call_service( + self.set_pin_value_service, + SetPinValue.Request(pin=str(pin), type="digital", value=value), ) return value.status @@ -543,7 +592,9 @@ def setMotorSpeed(self, motor, value): bool: True if set successfully. """ - motor = self._call_service(self.motor_services[motor], SetMotorSpeed.Request(speed=int(value))) + motor = self._call_service( + self.motor_services[motor], SetMotorSpeed.Request(speed=int(value)) + ) return motor.status def setMotorControl(self, status): @@ -558,9 +609,17 @@ def setMotorControl(self, status): none """ if status: - self._call_service(self.switch_controller_service, SwitchController.Request(activate_controllers=[self.CONTROLLER], activate_asap=True)) + self._call_service( + self.switch_controller_service, + SwitchController.Request( + activate_controllers=[self.CONTROLLER], activate_asap=True + ), + ) else: - self._call_service(self.switch_controller_service, SwitchController.Request(deactivate_controllers=[self.CONTROLLER])) + self._call_service( + self.switch_controller_service, + SwitchController.Request(deactivate_controllers=[self.CONTROLLER]), + ) return def stop(self): @@ -581,7 +640,9 @@ def _at_exit(self): # We need a special function to initiate the Robot() because the main.py need to call the # init_node() (see: https://answers.ros.org/question/266612/rospy-init_node-inside-imported-file/) -def createRobot(machine_namespace: Optional[str] = None, hardware_namespace: str = "io"): +def createRobot( + machine_namespace: Optional[str] = None, hardware_namespace: str = "io" +): """Creates and return instance of the robot class. Parameters: @@ -595,15 +656,3 @@ def createRobot(machine_namespace: Optional[str] = None, hardware_namespace: str global mirte mirte = Robot(machine_namespace, hardware_namespace) return mirte - - -if __name__ == "__main__": - rob = createRobot() - print("1") - rob.setServoAngle("right", 0) - time.sleep(0.5) - print("next") - rob.setServoAngle("right", 90) - - rob.setOLEDText("right", "laar") - From 11d7b9ae471475a6c43d6bc7b736ef1bfccdca7e Mon Sep 17 00:00:00 2001 From: SuperJappie08 <36795178+SuperJappie08@users.noreply.github.com> Date: Mon, 7 Oct 2024 09:08:33 +0200 Subject: [PATCH 10/34] Fix dependencies to make installable --- setup.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 8c6cd81..3745ab0 100644 --- a/setup.py +++ b/setup.py @@ -5,7 +5,8 @@ setuptools.setup( name="mirte_robot", - install_requires=["websocket_server", "rclpy"], # TODO: Require mirte_msgs? rcl_interfaces? controller_manager_msgs? + install_requires=["websocket_server"], # TODO: Require mirte_msgs? rcl_interfaces? controller_manager_msgs? + # Also requires "rclpy" version="0.1.0", author="Martin Klomp", author_email="m.klomp@tudelft.nl", From 67a32568bf22f2ff1a893c2942324d0aff33010b Mon Sep 17 00:00:00 2001 From: SuperJappie08 <36795178+SuperJappie08@users.noreply.github.com> Date: Wed, 9 Oct 2024 16:58:32 +0200 Subject: [PATCH 11/34] Update color sensor to HSL --- mirte_robot/robot.py | 62 +++++++++++++++++++++++++++++++++----------- 1 file changed, 47 insertions(+), 15 deletions(-) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index 5bedaf7..292ff0b 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -1,9 +1,8 @@ #!/usr/bin/env python -import time -from typing import TYPE_CHECKING, Optional import os - import platform +import time +from typing import TYPE_CHECKING, Optional import rclpy import rclpy.node @@ -14,7 +13,8 @@ from controller_manager_msgs.srv import SwitchController from mirte_msgs.srv import ( - GetColor, + GetColorHSL, + GetColorRGBW, GetDistance, GetEncoder, GetIntensity, @@ -308,18 +308,24 @@ def __init__( rclpy.spin_until_future_complete(self._node, color_future) - # FIXME: Add color sensor from v0.1.1 color_prefixes = color_future.result().result.prefixes if len(color_prefixes) > 0: color_sensors = [sensor.split(".")[-1] for sensor in color_prefixes] - self.color_services = {} + self.color_services: dict[str, dict[str, "rclpy.client.Client"]] = {} for sensor in color_sensors: self._node.get_logger().info( f"Created service client for color sensor [{sensor}]" ) - self.color_services[sensor] = self._node.create_client( - GetColor, self._hardware_namespace + "/get_color_" + sensor - ) + self.color_services[sensor] = { + "RGBW": self._node.create_client( + GetColorRGBW, + self._hardware_namespace + "/color/" + sensor + "/get_rgbw", + ), + "HSL": self._node.create_client( + GetColorHSL, + self._hardware_namespace + "/color/" + sensor + "/get_hsl", + ), + } self.get_pin_value_service = self._node.create_client( GetPinValue, self._hardware_namespace + "/get_pin_value" @@ -432,6 +438,26 @@ def getKeypad(self, keypad): return value.data # TODO: Is this implemented? Message might be missing + def getColorRGBW(self, sensor): + """Gets the value of the color sensor. + + Parameters: + sensor (str): The name of the sensor as defined in the configuration. + + Returns: + {r, g, b, w}: Scaled (0-1) values per R(ed), G(reen), B(lue), and W(hite). + """ + + value = self._call_service( + self.color_services[sensor]["RGBW"], GetColorRGBW.Request() + ) + return { + "r": value.color.r, + "g": value.color.g, + "b": value.color.b, + "w": value.color.w, + } + def getColor(self, sensor): """Gets the value of the color sensor. @@ -439,15 +465,16 @@ def getColor(self, sensor): sensor (str): The name of the sensor as defined in the configuration. Returns: - {r, g, b, w}: Raw (0-65536) values per R(ed), G(reen), B(lue), and W(hite). + {h, s, l}: Hue (0-360), Saturation (0-1), Lightness. """ - value = self._call_service(self.color_services[sensor], GetColor.Request())() + value = self._call_service( + self.color_services[sensor]["HSL"], GetColorHSL.Request() + ) return { - "r": value.color.color.r, - "g": value.color.color.g, - "b": value.color.color.b, - "w": value.color.color.w, + "h": value.color.h, + "s": value.color.s, + "l": value.color.l, } def getAnalogPinValue(self, pin): @@ -656,3 +683,8 @@ def createRobot( global mirte mirte = Robot(machine_namespace, hardware_namespace) return mirte + + +if __name__ == "__main__": + rob = createRobot() + print(rob.getColor("right")) From cb0bee261b1661cdf5485808737ceab71bfe4ee6 Mon Sep 17 00:00:00 2001 From: SuperJappie08 <36795178+SuperJappie08@users.noreply.github.com> Date: Mon, 14 Oct 2024 09:42:57 +0200 Subject: [PATCH 12/34] Migrate service convention --- mirte_robot/robot.py | 56 ++++++++++++++++++++++++++------------------ 1 file changed, 33 insertions(+), 23 deletions(-) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index 292ff0b..79e5dd8 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -2,6 +2,7 @@ import os import platform import time +import math from typing import TYPE_CHECKING, Optional import rclpy @@ -15,12 +16,12 @@ from mirte_msgs.srv import ( GetColorHSL, GetColorRGBW, - GetDistance, GetEncoder, GetIntensity, GetIntensityDigital, GetKeypad, GetPinValue, + GetRange, SetMotorSpeed, SetOLEDFile, SetOLEDText, @@ -135,7 +136,8 @@ def __init__( f"Created service client for motor [{motor}]" ) self.motor_services[motor] = self._node.create_client( - SetMotorSpeed, self._hardware_namespace + "/set_" + motor + "_speed" + SetMotorSpeed, + self._hardware_namespace + "/motor/" + motor + "/set_speed", ) # Service for motor speed @@ -156,7 +158,7 @@ def __init__( ) self.servo_services[servo] = self._node.create_client( SetServoAngle, - self._hardware_namespace + "/set_" + servo + "_servo_angle", + self._hardware_namespace + "/servo/" + servo + "/set_angle", ) ## Sensors @@ -182,7 +184,8 @@ def __init__( f"Created service client for distance sensor [{sensor}]" ) self.distance_services[sensor] = self._node.create_client( - GetDistance, self._hardware_namespace + "/get_distance_" + sensor + GetRange, + self._hardware_namespace + "/distance/" + sensor + "/get_range", ) # Services for oled @@ -203,11 +206,11 @@ def __init__( self.oled_services[oled] = { "text": self._node.create_client( SetOLEDText, - self._hardware_namespace + "/oled/set_" + oled + "_text", + self._hardware_namespace + "/oled/" + oled + "/set_text", ), "file": self._node.create_client( SetOLEDFile, - self._hardware_namespace + "/oled/set_" + oled + "_file", + self._hardware_namespace + "/oled/" + oled + "/set_file", ), } @@ -235,7 +238,7 @@ def __init__( ) for sensor in intensity_sensors: if ( - self._hardware_namespace + "/get_intensity_" + sensor + self._hardware_namespace + "/intensity/" + sensor + "/get_analog" in service_list ): self._node.get_logger().info( @@ -243,10 +246,13 @@ def __init__( ) self.intensity_services[sensor] = self._node.create_client( GetIntensity, - self._hardware_namespace + "/get_intensity_" + sensor, + self._hardware_namespace + + "/intensity/" + + sensor + + "/get_analog", ) if ( - self._hardware_namespace + "/get_intensity_" + sensor + "_digital" + self._hardware_namespace + "/intensity/" + sensor + "/get_digital" in service_list ): self._node.get_logger().info( @@ -256,9 +262,9 @@ def __init__( self._node.create_client( GetIntensityDigital, self._hardware_namespace - + "/get_intensity_" + + "/intensity/" + sensor - + "_digital", + + "/get_digital", ) ) @@ -279,7 +285,7 @@ def __init__( ) self.encoder_services[sensor] = self._node.create_client( GetEncoder, - self._hardware_namespace + "/get_encoder_" + sensor, + self._hardware_namespace + "/encoder/" + sensor + "/get_encoder", ) # Services for keypad sensors @@ -298,7 +304,8 @@ def __init__( f"Created service client for keypad [{sensor}]" ) self.keypad_services[sensor] = self._node.create_client( - GetKeypad, self._hardware_namespace + "/get_keypad_" + sensor + GetKeypad, + self._hardware_namespace + "/keypad/" + sensor + "/get_key", ) # Services for color sensors @@ -380,15 +387,24 @@ def getDistance(self, sensor): sensor (str): The name of the sensor as defined in the configuration. Returns: - int: Range in meters measured by the HC-SR04 sensor. + int: Range in meters measured by the HC-SR04 sensor. (The distance gets clamped to minimum and maximum range of the HC-SR04 sensor) Warning: A maximum of 6 distance sensors is supported. """ - dist = self._call_service(self.distance_services[sensor], GetDistance.Request()) - # FIXME: What to do about -inf, nan and inf? - return dist.data + value = self._call_service(self.distance_services[sensor], GetRange.Request()) + range = value.range + + distance: float = range.range + + # FIXME: What to do about nan? + if distance == math.inf: + distance = range.max_range + elif distance == -math.inf: + distance = range.min_range + + return distance def getIntensity(self, sensor, type="analog"): """Gets data from an intensity sensor. @@ -437,7 +453,6 @@ def getKeypad(self, keypad): value = self._call_service(self.keypad_services[keypad], GetKeypad.Request()) return value.data - # TODO: Is this implemented? Message might be missing def getColorRGBW(self, sensor): """Gets the value of the color sensor. @@ -683,8 +698,3 @@ def createRobot( global mirte mirte = Robot(machine_namespace, hardware_namespace) return mirte - - -if __name__ == "__main__": - rob = createRobot() - print(rob.getColor("right")) From 4fdb2db263a176e773cda3c08f7da008db771bb7 Mon Sep 17 00:00:00 2001 From: Jasper van Brakel <36795178+SuperJappie08@users.noreply.github.com> Date: Tue, 29 Oct 2024 12:30:50 +0100 Subject: [PATCH 13/34] Apply spelling suggestions from code review Co-authored-by: Arend-Jan van Hilten --- mirte_robot/robot.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index 79e5dd8..7133b7a 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -32,7 +32,7 @@ mirte = {} -# No QoS Profiles are not set, but this might not be required, since they might already behave like ROS 1 persistant. +# No QoS Profiles are set, but this might not be required, since they might already behave like ROS 1 persistant. class Robot: @@ -45,7 +45,7 @@ class Robot: # Implementation Notes: # This class creates a hidden ROS Node for the communication. # This should only be run once, however this can not be prevented from the web interface. - # There for the node is also anonymized with the current time. + # Therefore the node is also anonymized with the current time. def __init__( self, machine_namespace: Optional[str] = None, hardware_namespace: str = "io" @@ -350,7 +350,7 @@ def _call_service( rclpy.spin_until_future_complete(self._node, future_response) return future_response.result() - # FIXME: Check if services are avaiable, if not don't hard error on: + # FIXME: Check if services are available, if not don't hard error on: # AttributeError: 'Robot' object has no attribute 'oled_services'. Did you mean: '_services'? def _check_available(self, services: dict[str] | None, id: str) -> bool: if services is None: From d51a3e1719603e0915e4b1ccfa29204572632a14 Mon Sep 17 00:00:00 2001 From: SuperJappie08 <36795178+SuperJappie08@users.noreply.github.com> Date: Thu, 31 Oct 2024 16:21:16 +0100 Subject: [PATCH 14/34] Update documentation --- mirte_robot/robot.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index 7133b7a..14961d1 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -538,7 +538,10 @@ def setOLEDImage(self, oled, image): Parameters: oled (str): The name of the sensor as defined in the configuration. - image (str): Image name as defined in the images folder of the mirte-oled-images repository (optionally excl file extension). + image (str): Image name/path either an absolute path, a path + relative to the folder of the mirte-oled-images + repository or and package relative path (pkg://PACKAGE_NAME/REST/OF/PATH). + The image extension can be omitted if its png. """ value = self._call_service( @@ -554,7 +557,10 @@ def setOLEDAnimation(self, oled, animation): Parameters: oled (str): The name of the sensor as defined in the configuration. - animation (str): Animation (directory) name as defined in the animations folder of the mirte-oled-images repository. + animation (str): Animation (directory) name/path either an absolute + path, a path relative to the folder of the + mirte-oled-images repository or and package + relative path (pkg://PACKAGE_NAME/REST/OF/PATH). """ value = self._call_service( From 4e94f35b4bd0e7e13d833a253060456580956839 Mon Sep 17 00:00:00 2001 From: SuperJappie08 <36795178+SuperJappie08@users.noreply.github.com> Date: Thu, 31 Oct 2024 16:30:36 +0100 Subject: [PATCH 15/34] Add type hints --- mirte_robot/robot.py | 50 ++++++++++++++++++++++---------------------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index 14961d1..c2ae3c1 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -357,7 +357,7 @@ def _check_available(self, services: dict[str] | None, id: str) -> bool: return False return id in services - def getTimestamp(self): + def getTimestamp(self) -> float: """Gets the elapsed time in seconds since the initialization fo the Robot. Returns: @@ -366,7 +366,7 @@ def getTimestamp(self): return time.time() - self.begin_time - def getTimeSinceLastCall(self): + def getTimeSinceLastCall(self) -> float: """Gets the elapsed time in seconds since the last call to this function. Returns: @@ -380,14 +380,14 @@ def getTimeSinceLastCall(self): else: return time.time() - last_call - def getDistance(self, sensor): + def getDistance(self, sensor: str) -> float: """Gets data from a HC-SR04 distance sensor: calculated distance in meters. Parameters: sensor (str): The name of the sensor as defined in the configuration. Returns: - int: Range in meters measured by the HC-SR04 sensor. (The distance gets clamped to minimum and maximum range of the HC-SR04 sensor) + float: Range in meters measured by the HC-SR04 sensor. (The distance gets clamped to minimum and maximum range of the HC-SR04 sensor) Warning: A maximum of 6 distance sensors is supported. @@ -406,7 +406,7 @@ def getDistance(self, sensor): return distance - def getIntensity(self, sensor, type="analog"): + def getIntensity(self, sensor: str, type: str = "analog") -> int: """Gets data from an intensity sensor. Parameters: @@ -427,7 +427,7 @@ def getIntensity(self, sensor, type="analog"): ) return value.data - def getEncoder(self, sensor): + def getEncoder(self, sensor: str) -> int: """Gets data from an encoder: every encoder pulse increments the counter. Parameters: @@ -440,7 +440,7 @@ def getEncoder(self, sensor): value = self._call_service(self.encoder_services[sensor], GetEncoder.Request()) return value.data - def getKeypad(self, keypad): + def getKeypad(self, keypad: str) -> str: """Gets the value of the keypad: the button that is pressed. Parameters: @@ -453,7 +453,7 @@ def getKeypad(self, keypad): value = self._call_service(self.keypad_services[keypad], GetKeypad.Request()) return value.data - def getColorRGBW(self, sensor): + def getColorRGBW(self, sensor: str) -> dict[str, float]: """Gets the value of the color sensor. Parameters: @@ -473,7 +473,7 @@ def getColorRGBW(self, sensor): "w": value.color.w, } - def getColor(self, sensor): + def getColor(self, sensor: str) -> dict[str, float]: """Gets the value of the color sensor. Parameters: @@ -492,7 +492,7 @@ def getColor(self, sensor): "l": value.color.l, } - def getAnalogPinValue(self, pin): + def getAnalogPinValue(self, pin: str) -> int: """Gets the input value of an analog pin. Parameters: @@ -507,7 +507,7 @@ def getAnalogPinValue(self, pin): ) return value.data - def setAnalogPinValue(self, pin, value): + def setAnalogPinValue(self, pin: str, value: int) -> bool: """Sets the output value of an analog pin (PWM). Parameters: @@ -521,7 +521,7 @@ def setAnalogPinValue(self, pin, value): ) return value.status - def setOLEDText(self, oled, text): + def setOLEDText(self, oled: str, text: str) -> bool: """Shows text on the OLED. Parameters: @@ -533,7 +533,7 @@ def setOLEDText(self, oled, text): ) return value.status - def setOLEDImage(self, oled, image): + def setOLEDImage(self, oled: str, image: str) -> bool: """Shows image on the OLED. Parameters: @@ -552,7 +552,7 @@ def setOLEDImage(self, oled, image): ) return value.status - def setOLEDAnimation(self, oled, animation): + def setOLEDAnimation(self, oled: str, animation: str) -> bool: """Shows animation on the OLED. Parameters: @@ -568,11 +568,11 @@ def setOLEDAnimation(self, oled, animation): ) return value.status - def getDigitalPinValue(self, pin): + def getDigitalPinValue(self, pin: str) -> bool: """Gets the input value of a digital pin. Parameters: - pin (str): The pin number of an analog pin as printed on the microcontroller. + pin (str): The pin number of a digital pin as printed on the microcontroller. Returns: bool: The input value. @@ -582,14 +582,14 @@ def getDigitalPinValue(self, pin): self.get_pin_value_service, GetPinValue.Request(pin=str(pin), type="digital"), ) - return value.data + return bool(value.data) - def setServoAngle(self, servo, angle): + def setServoAngle(self, servo: str, angle: float) -> bool: """Sets the angle of a servo. Parameters: servo (str): The name of the sensor as defined in the configuration. - angle (int): The angle of the servo (range [0-360], but some servos + angle (float): The angle of the servo (range [0-360], but some servos might be physically limited to [0-180]. Returns: @@ -614,7 +614,7 @@ def setServoAngle(self, servo, angle): ) return value.status - def setDigitalPinValue(self, pin, value): + def setDigitalPinValue(self, pin: str, value: bool) -> bool: """Sets the output value of a digital pin. Parameters: @@ -628,7 +628,7 @@ def setDigitalPinValue(self, pin, value): ) return value.status - def setMotorSpeed(self, motor, value): + def setMotorSpeed(self, motor: str, value: int) -> bool: """Sets the speed of the motor. Parameters: @@ -645,7 +645,7 @@ def setMotorSpeed(self, motor, value): ) return motor.status - def setMotorControl(self, status): + def setMotorControl(self, status: bool) -> None: """Enables/disables the motor controller. This is enabled on boot, but can be disabled/enabled at runtime. This makes the ROS control node pause, so it will not respond to Twist messages anymore when disabled. @@ -670,7 +670,7 @@ def setMotorControl(self, status): ) return - def stop(self): + def stop(self) -> None: """Stops all DC motors defined in the configuration Note: @@ -681,7 +681,7 @@ def stop(self): for motor in self.motors: self.setMotorSpeed(motor, 0) - def _at_exit(self): + def _at_exit(self) -> None: self.stop() rclpy.try_shutdown() @@ -690,7 +690,7 @@ def _at_exit(self): # init_node() (see: https://answers.ros.org/question/266612/rospy-init_node-inside-imported-file/) def createRobot( machine_namespace: Optional[str] = None, hardware_namespace: str = "io" -): +) -> Robot: """Creates and return instance of the robot class. Parameters: From a1815e0de972895c662004542c3d7c920d5f3fa9 Mon Sep 17 00:00:00 2001 From: SuperJappie08 <36795178+SuperJappie08@users.noreply.github.com> Date: Thu, 7 Nov 2024 17:05:05 +0100 Subject: [PATCH 16/34] Change Pin Services --- mirte_robot/robot.py | 215 ++++++++++++++++++++++++++++++++++--------- 1 file changed, 171 insertions(+), 44 deletions(-) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index c2ae3c1..37723e1 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -1,9 +1,10 @@ #!/usr/bin/env python +import math import os import platform import time -import math -from typing import TYPE_CHECKING, Optional +import weakref +from typing import TYPE_CHECKING, Literal, Optional, overload import rclpy import rclpy.node @@ -14,18 +15,21 @@ from controller_manager_msgs.srv import SwitchController from mirte_msgs.srv import ( + GetAnalogPinValue, + GetBoardCharacteristics, GetColorHSL, GetColorRGBW, + GetDigitalPinValue, GetEncoder, GetIntensity, GetIntensityDigital, GetKeypad, - GetPinValue, GetRange, + SetDigitalPinValue, SetMotorSpeed, SetOLEDFile, SetOLEDText, - SetPinValue, + SetPWMPinValue, SetServoAngle, ) from rcl_interfaces.srv import ListParameters @@ -101,7 +105,7 @@ def __init__( # Call controller_manager/switch_controller service to disable/enable the ROS diff_drive_controller # By default this class will control the robot though PWM (controller stopped). Only in case # the controller is needed, it will be enabled. - self.switch_controller_service = self._node.create_client( + self._switch_controller_service = self._node.create_client( SwitchController, "controller_manager/switch_controller" ) @@ -111,13 +115,39 @@ def __init__( self._hardware_namespace + "telemetrix", 10 ): self._node.get_logger().fatal( - f"Telemetrix node at '{self._node.get_namespace() + self._hardware_namespace + 'telemetrix'}' was not found! Aborting" + f"Telemetrix node at '{self._node.get_namespace() + '/' + self._hardware_namespace + '/telemetrix'}' was not found! Aborting" ) exit(-1) list_parameters: rclpy.client.Client = self._node.create_client( ListParameters, self._hardware_namespace + "/telemetrix/list_parameters" ) + # # Get the Board Characteristics + get_board_characteristics = self._node.create_client( + GetBoardCharacteristics, + self._hardware_namespace + "/get_board_characteristics", + ) + + # Wait for the get_board_characteristics to prevent weird errors. + if not get_board_characteristics.wait_for_service(1): + self._node.get_logger().fatal( + f"Telemetrix node at '{self._node.get_namespace()+ '/'+ self._hardware_namespace + '/telemetrix'}' does not provide a '{self._node.get_namespace() + '/' + self._hardware_namespace + '/get_board_characteristics'}' service! Aborting" + ) + exit(-1) + + board_future: rclpy.Future = get_board_characteristics.call_async( + GetBoardCharacteristics.Request() + ) + rclpy.spin_until_future_complete(self._node, board_future) + + board_characteristics: GetBoardCharacteristics.Response = board_future.result() + + self._max_adc: int = board_characteristics.max_adc + self._max_pwm: int = board_characteristics.max_pwm + self._max_voltage: float = board_characteristics.max_voltage + + self._node.destroy_client(get_board_characteristics) + # Service for motor speed self.motors = {} # FIXME: Is self.motors used? motors_future: rclpy.Future = list_parameters.call_async( @@ -142,6 +172,15 @@ def __init__( # Service for motor speed + def finalize(node: rclpy.node.Node, motors: dict[str, rclpy.client.Client]): + for motor in motors.values(): + future = motor.call_async(SetMotorSpeed.Request(speed=0)) + rclpy.spin_until_future_complete(node, future) + + self._finalizer = weakref.finalize( + self, finalize, self._node, self.motor_services + ) + servo_future = list_parameters.call_async( ListParameters.Request(prefixes=["servo"], depth=3) ) @@ -334,11 +373,22 @@ def __init__( ), } - self.get_pin_value_service = self._node.create_client( - GetPinValue, self._hardware_namespace + "/get_pin_value" + self._node.destroy_client(list_parameters) + + self._get_digital_pin_value_service = self._node.create_client( + GetDigitalPinValue, self._hardware_namespace + "/get_digital_pin_value" + ) + + self._get_analog_pin_value_service = self._node.create_client( + GetAnalogPinValue, self._hardware_namespace + "/get_analog_pin_value" + ) + + self._set_digital_pin_value_service = self._node.create_client( + SetDigitalPinValue, self._hardware_namespace + "/set_digital_pin_value" ) - self.set_pin_value_service = self._node.create_client( - SetPinValue, self._hardware_namespace + "/set_pin_value" + + self._set_pwm_pin_value_service = self._node.create_client( + SetPWMPinValue, self._hardware_namespace + "/set_pwm_pin_value" ) def _call_service( @@ -406,7 +456,15 @@ def getDistance(self, sensor: str) -> float: return distance - def getIntensity(self, sensor: str, type: str = "analog") -> int: + # TODO: Maybe change digital return type to bool + @overload + def getIntensity(self, sensor: str, type: Literal["analog"]) -> float: ... + @overload + def getIntensity(self, sensor: str, type: Literal["digital"]) -> int: ... + + def getIntensity( + self, sensor: str, type: Literal["analog", "digital"] = "analog" + ) -> int | float: """Gets data from an intensity sensor. Parameters: @@ -414,8 +472,9 @@ def getIntensity(self, sensor: str, type: str = "analog") -> int: type (str): The type of the sensor (either 'analog' or 'digital'). Returns: - int: Value of the sensor (0-255 when analog, 0-1 when digital). + int | float: Value of the sensor (0.0-1.0 when analog, 0-1 when digital). """ + # FIXME: IMPROVE ERROR for type if type == "analog": value = self._call_service( self.intensity_services[sensor], GetIntensity.Request() @@ -440,7 +499,9 @@ def getEncoder(self, sensor: str) -> int: value = self._call_service(self.encoder_services[sensor], GetEncoder.Request()) return value.data - def getKeypad(self, keypad: str) -> str: + def getKeypad( + self, keypad: str + ) -> Literal["", "up", "down", "left", "right", "enter"]: """Gets the value of the keypad: the button that is pressed. Parameters: @@ -492,22 +553,69 @@ def getColor(self, sensor: str) -> dict[str, float]: "l": value.color.l, } - def getAnalogPinValue(self, pin: str) -> int: + @overload # FIXME: Should this return int of float + def getAnalogPinValue(self, pin: str, mode: Literal["percentage"]) -> int: ... + + @overload + def getAnalogPinValue(self, pin: str, mode: Literal["raw"]) -> int: ... + + @overload + def getAnalogPinValue(self, pin: str, mode: Literal["voltage"]) -> float: ... + + # TODO: What to do with max? + def getAnalogPinValue( + self, pin: str, mode: Literal["percentage", "raw", "voltage"] = "percentage" + ) -> float: """Gets the input value of an analog pin. Parameters: pin (str): The pin number of an analog pin as printed on the microcontroller. + mode (str, optional): The units of the value, can be "percentage", "raw" or "voltage". Defaults to "percentage". + # FIXME: HOW TO DO DOCS Returns: - int: Value between 0-255. + int: RAW + float: Value between 0-5V (Arduino) or 0-3.3V (Pico). """ - value = self._call_service( - self.get_pin_value_service, GetPinValue.Request(pin=str(pin), type="analog") + response: GetAnalogPinValue.Response = self._call_service( + self._get_analog_pin_value_service, + GetAnalogPinValue.Request(pin=str(pin)), ) - return value.data - def setAnalogPinValue(self, pin: str, value: int) -> bool: + # FIXME: TMP CHECK + assert response.status, response.message + match mode: + case "raw": + return response.value + case "percentage": + return response.value / self._max_adc * 100 + case "voltage": + return response.value / self._max_adc * self._max_voltage + case _: + assert False, "FIXME: UNKNOWN MODE" + + # TODO: Input int or float? Why not both + @overload + def setAnalogPinValue( + self, pin: str, value: int | float, mode: Literal["percentage"] + ) -> bool: ... + + @overload + def setAnalogPinValue(self, pin: str, value: int, mode: Literal["raw"]) -> bool: ... + + @overload + def setAnalogPinValue( + self, pin: str, value: float, mode: Literal["voltage"] + ) -> bool: ... + + # FIXME: UPDATE DOCS + def setAnalogPinValue( + self, + pin: str, + value: int | float, + mode: Literal["percentage", "raw", "voltage"] = "percentage", + ) -> bool: """Sets the output value of an analog pin (PWM). Parameters: @@ -515,11 +623,29 @@ def setAnalogPinValue(self, pin: str, value: int) -> bool: value (int): Value between 0-255. """ - value = self._call_service( - self.set_pin_value_service, - SetPinValue.Request(pin=str(pin), type="analog", value=value), + raw_value = None + + match mode: + # TODO: Maybe add u16 bounds checking, since message type is picky + case "raw": + # Bounds checking happens in telemetrix node + raw_value = int(value) + case "percentage": + # TODO: Maybe check percentage bounds here for better error. + raw_value = int((value / 100) * self._max_pwm) + case "voltage": + # TODO: Maybe check voltage bounds here for better error. + raw_value = int((value / self._max_voltage) * self._max_pwm) + case _: + assert False, "NO VALID MODE" + + response: SetPWMPinValue.Response = self._call_service( + self._set_pwm_pin_value_service, + SetPWMPinValue.Request(pin=str(pin), value=raw_value), ) - return value.status + + assert response.status, response.message + return response.status def setOLEDText(self, oled: str, text: str) -> bool: """Shows text on the OLED. @@ -578,11 +704,12 @@ def getDigitalPinValue(self, pin: str) -> bool: bool: The input value. """ - value = self._call_service( - self.get_pin_value_service, - GetPinValue.Request(pin=str(pin), type="digital"), + response: GetDigitalPinValue.Response = self._call_service( + self._get_digital_pin_value_service, + GetDigitalPinValue.Request(pin=str(pin)), ) - return bool(value.data) + assert response.status, response.message + return bool(response.value) def setServoAngle(self, servo: str, angle: float) -> bool: """Sets the angle of a servo. @@ -622,11 +749,13 @@ def setDigitalPinValue(self, pin: str, value: bool) -> bool: value (bool): Value to set. """ - value = self._call_service( - self.set_pin_value_service, - SetPinValue.Request(pin=str(pin), type="digital", value=value), + response: SetDigitalPinValue.Response = self._call_service( + self._set_digital_pin_value_service, + SetDigitalPinValue.Request(pin=str(pin), value=value), ) - return value.status + assert response.status, response.message # FIXME: TMP ERROR + + return response.status def setMotorSpeed(self, motor: str, value: int) -> bool: """Sets the speed of the motor. @@ -645,7 +774,7 @@ def setMotorSpeed(self, motor: str, value: int) -> bool: ) return motor.status - def setMotorControl(self, status: bool) -> None: + def setMotorControl(self, status: bool) -> bool: """Enables/disables the motor controller. This is enabled on boot, but can be disabled/enabled at runtime. This makes the ROS control node pause, so it will not respond to Twist messages anymore when disabled. @@ -654,21 +783,20 @@ def setMotorControl(self, status: bool) -> None: status (bool): To which status the motor controller should be set. Returns: - none + bool: True if succes (ok) """ + request = None if status: - self._call_service( - self.switch_controller_service, - SwitchController.Request( - activate_controllers=[self.CONTROLLER], activate_asap=True - ), + request = SwitchController.Request( + activate_controllers=[self.CONTROLLER], activate_asap=True ) else: - self._call_service( - self.switch_controller_service, - SwitchController.Request(deactivate_controllers=[self.CONTROLLER]), - ) - return + request = SwitchController.Request(deactivate_controllers=[self.CONTROLLER]) + + response: SwitchController.Response = self._call_service( + self._switch_controller_service, request + ) + return response.ok def stop(self) -> None: """Stops all DC motors defined in the configuration @@ -683,7 +811,6 @@ def stop(self) -> None: def _at_exit(self) -> None: self.stop() - rclpy.try_shutdown() # We need a special function to initiate the Robot() because the main.py need to call the From 8a4b87525cb93cbab562af3355cd977f9d5695aa Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Tue, 18 Mar 2025 21:56:59 +0000 Subject: [PATCH 17/34] Fixes to get web interface working --- mirte_robot/robot.py | 50 +++++++++++++++++++++++++++++++------------- 1 file changed, 35 insertions(+), 15 deletions(-) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index 37723e1..54c1ec5 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -3,6 +3,8 @@ import os import platform import time +import signal +import sys import weakref from typing import TYPE_CHECKING, Literal, Optional, overload @@ -38,7 +40,17 @@ # No QoS Profiles are set, but this might not be required, since they might already behave like ROS 1 persistant. +def singleton(cls): + instances = {} + def get_instance(*args, **kwargs): + if cls not in instances: + instances[cls] = cls(*args, **kwargs) + return instances[cls] + + return get_instance + +@singleton class Robot: """Robot API @@ -60,20 +72,20 @@ def __init__( machine_namespace (Optional[str], optional): The Namespace from '/' to the ROS namespace for the specific Mirte. Defaults to "/{HOSTNAME}". (This only has to be changed when running the Robot API from a different machine directly. It is configured correctly for the Web interface) hardware_namespace (str, optional): The namespace for the hardware peripherals. Defaults to "io". """ - self._machine_namespace = ( - machine_namespace - if machine_namespace and validate_namespace(machine_namespace) - else "/" + platform.node().replace("-", "_").lower() - ) - self._hardware_namespace = ( - hardware_namespace - if validate_namespace( - hardware_namespace - if hardware_namespace.startswith("/") - else (self._machine_namespace + "/" + hardware_namespace) - ) - else "io" - ) + self._machine_namespace = "" #( +# machine_namespace +# if machine_namespace and validate_namespace(machine_namespace) +# else "/" + platform.node().replace("-", "_").lower() +# ) + self._hardware_namespace = "/io" #( +# hardware_namespace +# if validate_namespace( +# hardware_namespace +# if hardware_namespace.startswith("/") +# else (self._machine_namespace + "/" + hardware_namespace) +# ) +# else "io" +# ) ROS_DISTRO = os.getenv("ROS_DISTRO") @@ -129,7 +141,7 @@ def __init__( ) # Wait for the get_board_characteristics to prevent weird errors. - if not get_board_characteristics.wait_for_service(1): + if not get_board_characteristics.wait_for_service(2): self._node.get_logger().fatal( f"Telemetrix node at '{self._node.get_namespace()+ '/'+ self._hardware_namespace + '/telemetrix'}' does not provide a '{self._node.get_namespace() + '/' + self._hardware_namespace + '/get_board_characteristics'}' service! Aborting" ) @@ -391,6 +403,9 @@ def finalize(node: rclpy.node.Node, motors: dict[str, rclpy.client.Client]): SetPWMPinValue, self._hardware_namespace + "/set_pwm_pin_value" ) + signal.signal(signal.SIGINT, self._signal_handler) + signal.signal(signal.SIGTERM, self._signal_handler) + def _call_service( self, client: rclpy.client.Client, request: rclpy.client.SrvTypeRequest ) -> rclpy.client.SrvTypeResponse: @@ -809,12 +824,17 @@ def stop(self) -> None: for motor in self.motors: self.setMotorSpeed(motor, 0) + def _signal_handler(self, sig, frame): + self._at_exit() + def _at_exit(self) -> None: self.stop() + sys.exit() # We need a special function to initiate the Robot() because the main.py need to call the # init_node() (see: https://answers.ros.org/question/266612/rospy-init_node-inside-imported-file/) +# TODO: We probably do not need this anymore in ROS2. But it affects the python imports. def createRobot( machine_namespace: Optional[str] = None, hardware_namespace: str = "io" ) -> Robot: From 5e290fbca391f20c2ed867fc4ac7d40473b9b1ca Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Tue, 15 Apr 2025 16:04:32 +0200 Subject: [PATCH 18/34] Fixes for sphinx autodoc --- mirte_robot/robot.py | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index 54c1ec5..10f7e34 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -50,7 +50,8 @@ def get_instance(*args, **kwargs): return get_instance -@singleton +# We should not decorate the class here with @singleton. That will +# prevent sphinx autodoc from generating the docs for this class. class Robot: """Robot API @@ -66,12 +67,12 @@ class Robot: def __init__( self, machine_namespace: Optional[str] = None, hardware_namespace: str = "io" ): - """Intialize the Mirte Robot API + """Intialize the Mirte Robot API""" + +# Parameters: +# machine_namespace (Optional[str], optional): The Namespace from '/' to the ROS namespace for the specific Mirte. Defaults to "/{HOSTNAME}". (This only has to be changed when running the Robot API from a different machine directly. It is configured correctly for the Web interface) +# hardware_namespace (str, optional): The namespace for the hardware peripherals. Defaults to "io". - Parameters: - machine_namespace (Optional[str], optional): The Namespace from '/' to the ROS namespace for the specific Mirte. Defaults to "/{HOSTNAME}". (This only has to be changed when running the Robot API from a different machine directly. It is configured correctly for the Web interface) - hardware_namespace (str, optional): The namespace for the hardware peripherals. Defaults to "io". - """ self._machine_namespace = "" #( # machine_namespace # if machine_namespace and validate_namespace(machine_namespace) @@ -587,10 +588,8 @@ def getAnalogPinValue( pin (str): The pin number of an analog pin as printed on the microcontroller. mode (str, optional): The units of the value, can be "percentage", "raw" or "voltage". Defaults to "percentage". - # FIXME: HOW TO DO DOCS Returns: - int: RAW - float: Value between 0-5V (Arduino) or 0-3.3V (Pico). + int | float: Value of the pin (0-100 when percentage, 0- when raw, 0-V when voltage). """ response: GetAnalogPinValue.Response = self._call_service( @@ -849,5 +848,5 @@ def createRobot( """ global mirte - mirte = Robot(machine_namespace, hardware_namespace) + mirte = singleton(Robot(machine_namespace, hardware_namespace)) return mirte From c61a1b4b9644f10da52699bdb4239f76756d3cec Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Tue, 15 Apr 2025 16:42:34 +0200 Subject: [PATCH 19/34] Added getROSNode to interface --- mirte_robot/robot.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index 10f7e34..be1c181 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -823,6 +823,9 @@ def stop(self) -> None: for motor in self.motors: self.setMotorSpeed(motor, 0) + def getROSNode(self): + return self._node + def _signal_handler(self, sig, frame): self._at_exit() From e6e9e9d9695c5c22f50603fcbda11e94a179eae6 Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Wed, 23 Apr 2025 20:54:45 +0200 Subject: [PATCH 20/34] Changed to Apache2 license --- LICENSE | 229 ++++++++++++++++++++++++++++++++++++++++++++++++------- setup.py | 3 +- 2 files changed, 203 insertions(+), 29 deletions(-) diff --git a/LICENSE b/LICENSE index fc45ba6..307b1ab 100644 --- a/LICENSE +++ b/LICENSE @@ -1,28 +1,201 @@ -BSD 3-Clause License - -Copyright (c) 2024, Martin Klomp, MIRTE-team - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -1. Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -3. Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright 2025, TU Delft + + 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 + + http://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/setup.py b/setup.py index 3745ab0..860ea8e 100644 --- a/setup.py +++ b/setup.py @@ -15,9 +15,10 @@ long_description_content_type="text/markdown", url="https://github.com/mirte-robot/mirte-python", packages=['mirte_robot'], + license="Apache License 2.0", classifiers=[ "Programming Language :: Python :: 3", - "License :: OSI Approved :: BSD License", + "License :: OSI Approved :: pache Software License", "Operating System :: OS Independent", ], python_requires='>=3.6', From e70a7c50de3c165028d00020ab8a9cbec740cf84 Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Wed, 30 Apr 2025 15:01:49 +0000 Subject: [PATCH 21/34] Fixed play button issues --- mirte_robot/robot.py | 47 ++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 43 insertions(+), 4 deletions(-) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index 54c1ec5..ece490d 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -5,6 +5,7 @@ import time import signal import sys +import threading import weakref from typing import TYPE_CHECKING, Literal, Optional, overload @@ -36,6 +37,18 @@ ) from rcl_interfaces.srv import ListParameters +##### +# +# There are 4 use cases that should exit the user code: +# +# 1) Running "while true" (and CTRL-C) from commandline +# 2) Running "for 10" from commandline +# 3) Running "while true" (and stop/CTRL-C) from web interface +# 4) Running "for 10" from web interface +# +##### + + mirte = {} # No QoS Profiles are set, but this might not be required, since they might already behave like ROS 1 persistant. @@ -101,6 +114,8 @@ def __init__( # Stop robot when exited rclpy.get_default_context().on_shutdown(self._at_exit) + self._stopping = False + self._lock = threading.Lock() self.CONTROLLER = "diffbot_base_controller" @@ -181,6 +196,7 @@ def __init__( SetMotorSpeed, self._hardware_namespace + "/motor/" + motor + "/set_speed", ) + self.motor_services[motor].wait_for_service() # Service for motor speed @@ -211,6 +227,7 @@ def finalize(node: rclpy.node.Node, motors: dict[str, rclpy.client.Client]): SetServoAngle, self._hardware_namespace + "/servo/" + servo + "/set_angle", ) + self.servo_services[servo].wait_for_service() ## Sensors ## The sensors are now just using a blocking service call. This is intentionally @@ -238,6 +255,7 @@ def finalize(node: rclpy.node.Node, motors: dict[str, rclpy.client.Client]): GetRange, self._hardware_namespace + "/distance/" + sensor + "/get_range", ) + self.distance_services[sensor].wait_for_service() # Services for oled oled_future = list_parameters.call_async( @@ -264,6 +282,8 @@ def finalize(node: rclpy.node.Node, motors: dict[str, rclpy.client.Client]): self._hardware_namespace + "/oled/" + oled + "/set_file", ), } + self.oled_services[oled]["text"].wait_for_service() + self.oled_services[oled]["file"].wait_for_service() # Services for intensity sensors (TODO: how to expose the digital version?) intensity_future = list_parameters.call_async( @@ -302,6 +322,7 @@ def finalize(node: rclpy.node.Node, motors: dict[str, rclpy.client.Client]): + sensor + "/get_analog", ) + self.intensity_services[sensor].wait_for_service() if ( self._hardware_namespace + "/intensity/" + sensor + "/get_digital" in service_list @@ -318,6 +339,7 @@ def finalize(node: rclpy.node.Node, motors: dict[str, rclpy.client.Client]): + "/get_digital", ) ) + self.intensity_services[sensor + "_digital"].wait_for_service() # Services for encoder sensors encoder_future = list_parameters.call_async( @@ -338,6 +360,7 @@ def finalize(node: rclpy.node.Node, motors: dict[str, rclpy.client.Client]): GetEncoder, self._hardware_namespace + "/encoder/" + sensor + "/get_encoder", ) + self.encoder_services[sensor].wait_for_service() # Services for keypad sensors keypad_future = list_parameters.call_async( @@ -358,6 +381,7 @@ def finalize(node: rclpy.node.Node, motors: dict[str, rclpy.client.Client]): GetKeypad, self._hardware_namespace + "/keypad/" + sensor + "/get_key", ) + self.keypad_services[sensor].wait_for_service() # Services for color sensors color_future = list_parameters.call_async( @@ -384,6 +408,9 @@ def finalize(node: rclpy.node.Node, motors: dict[str, rclpy.client.Client]): self._hardware_namespace + "/color/" + sensor + "/get_hsl", ), } + self.color_services[sensor]["RGBW"].wait_for_service() + self.color_services[sensor]["HSL"].wait_for_service() + self._node.destroy_client(list_parameters) @@ -409,10 +436,17 @@ def finalize(node: rclpy.node.Node, motors: dict[str, rclpy.client.Client]): def _call_service( self, client: rclpy.client.Client, request: rclpy.client.SrvTypeRequest ) -> rclpy.client.SrvTypeResponse: - client.wait_for_service() - future_response = client.call_async(request) - rclpy.spin_until_future_complete(self._node, future_response) + with self._lock: + future_response = client.call_async(request) + while not future_response.done() and not self._stopping: + rclpy.spin_once(self._node, timeout_sec=0.1) + + if self._stopping: + with self._lock: + self._stopping = False + self._at_exit() + return future_response.result() # FIXME: Check if services are available, if not don't hard error on: @@ -824,8 +858,13 @@ def stop(self) -> None: for motor in self.motors: self.setMotorSpeed(motor, 0) + def _signal_handler(self, sig, frame): - self._at_exit() + self._stopping = True + + if (not self._lock.locked()): + self._at_exit() + def _at_exit(self) -> None: self.stop() From 72e11c7fe5c47b1063b0b00cc698b85c9ab75595 Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Wed, 30 Apr 2025 18:16:45 +0000 Subject: [PATCH 22/34] Fixed singleton issue --- mirte_robot/robot.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index 9836d95..67e20f9 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -63,8 +63,10 @@ def get_instance(*args, **kwargs): return get_instance -# We should not decorate the class here with @singleton. That will -# prevent sphinx autodoc from generating the docs for this class. +# TODO: We should not decorate the class here with @singleton. That will +# prevent sphinx autodoc from generating the docs for this class. But the +# previous check did not work. +@singleton class Robot: """Robot API @@ -890,5 +892,5 @@ def createRobot( """ global mirte - mirte = singleton(Robot(machine_namespace, hardware_namespace)) + mirte = Robot(machine_namespace, hardware_namespace) return mirte From 14c73e1d3d1ed912abc7cb0061e7cc2f068460e0 Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Wed, 30 Apr 2025 18:23:26 +0000 Subject: [PATCH 23/34] Make distance inf, -inf and nan to -1 --- mirte_robot/robot.py | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index 67e20f9..aae8458 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -500,11 +500,8 @@ def getDistance(self, sensor: str) -> float: distance: float = range.range - # FIXME: What to do about nan? - if distance == math.inf: - distance = range.max_range - elif distance == -math.inf: - distance = range.min_range + if distance == math.inf or distance == -math.inf or math.isnan(distance): + distance = -1 return distance From 6efa753974a41c3be7ac082ed7213c56c19d4781 Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Mon, 12 May 2025 07:07:11 +0000 Subject: [PATCH 24/34] Refactor of singleton --- mirte_robot/robot.py | 42 +++++++++++++++++++++--------------------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index aae8458..b2cfff9 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -51,22 +51,6 @@ mirte = {} -# No QoS Profiles are set, but this might not be required, since they might already behave like ROS 1 persistant. - -def singleton(cls): - instances = {} - - def get_instance(*args, **kwargs): - if cls not in instances: - instances[cls] = cls(*args, **kwargs) - return instances[cls] - - return get_instance - -# TODO: We should not decorate the class here with @singleton. That will -# prevent sphinx autodoc from generating the docs for this class. But the -# previous check did not work. -@singleton class Robot: """Robot API @@ -76,14 +60,30 @@ class Robot: # Implementation Notes: # This class creates a hidden ROS Node for the communication. - # This should only be run once, however this can not be prevented from the web interface. + # This class is a singleton. + # No QoS Profiles are set, but this might not be required, since they might already behave like ROS 1 persistant. # Therefore the node is also anonymized with the current time. + # Ensuring singleton behavior + # This should not be done as a decorator, since the decorator returns a + # function. Therefor the sphynx documentation is not able to process + # this class anymore. + _instance = None + def __new__(cls, *args, **kwargs): + if cls._instance is None: + cls._instance = super(Robot, cls).__new__(cls) + rclpy.init() + return cls._instance + def __init__( self, machine_namespace: Optional[str] = None, hardware_namespace: str = "io" ): """Intialize the Mirte Robot API""" + # Only the first instance (singelton) needs to be initialized + if getattr(self, "_initialized", False): + return + # Parameters: # machine_namespace (Optional[str], optional): The Namespace from '/' to the ROS namespace for the specific Mirte. Defaults to "/{HOSTNAME}". (This only has to be changed when running the Robot API from a different machine directly. It is configured correctly for the Web interface) # hardware_namespace (str, optional): The namespace for the hardware peripherals. Defaults to "io". @@ -105,10 +105,8 @@ def __init__( ROS_DISTRO = os.getenv("ROS_DISTRO") - rclpy.init() - - # This node should be only ran once. - # No 'anonymous' flag available, so use unix epoch nano seconds to pad name + # In order to be able to run this from multiple python files (ie creating + # multtiple nodes) a timestamp is added (replicating a 'anonymous' node) self._node = rclpy.node.Node( "_mirte_python_api_" + str(time.time_ns()), namespace=self._machine_namespace, @@ -436,6 +434,8 @@ def finalize(node: rclpy.node.Node, motors: dict[str, rclpy.client.Client]): signal.signal(signal.SIGINT, self._signal_handler) signal.signal(signal.SIGTERM, self._signal_handler) + self._initialized = True + def _call_service( self, client: rclpy.client.Client, request: rclpy.client.SrvTypeRequest ) -> rclpy.client.SrvTypeResponse: From e81040d0594213fbfd8df836ddcd37896a30a99e Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Mon, 12 May 2025 09:23:39 +0000 Subject: [PATCH 25/34] Fix for setMotorControl() not working --- mirte_robot/robot.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index b2cfff9..2ba3cf1 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -118,7 +118,10 @@ def __init__( self._stopping = False self._lock = threading.Lock() - self.CONTROLLER = "diffbot_base_controller" + # Assuming we are only using the Python API for the MIRTE Pioneer. This + # is only used for setMotorControl(), so all other functions should + # (theoretically) still work on teh MIRTE Master. + self.CONTROLLER = "mirte_base_controller" self.PWM = 3 # PrivateConstants.PWM when moving to Python3 self.INPUT = 0 From 09d690d5cc40c1ea1aa8315d8cd97e84ad9e21c6 Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Mon, 19 May 2025 10:29:51 +0200 Subject: [PATCH 26/34] Added python style check. --- .github/workflows/python-check.yml | 11 ++++ README.md | 13 +++- mirte_robot/linetrace.py | 95 ++++++++++++++++-------------- mirte_robot/robot.py | 57 +++++++++--------- 4 files changed, 102 insertions(+), 74 deletions(-) create mode 100644 .github/workflows/python-check.yml diff --git a/.github/workflows/python-check.yml b/.github/workflows/python-check.yml new file mode 100644 index 0000000..e43522f --- /dev/null +++ b/.github/workflows/python-check.yml @@ -0,0 +1,11 @@ +name: python style check +on: [push, pull_request] +jobs: + check-python-style: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - name: Black Code Formatter + uses: lgeiger/black-action@master + with: + args: ". --check --diff" \ No newline at end of file diff --git a/README.md b/README.md index 824df0a..26f08e9 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,12 @@ -# Mirte Python API +# MIRTE Python API +This package provides the API for the MIRTE Robot. Please +read the MIRTE documentation. -This package provides the API for the Mirte Robot. Please read the Mirte documentation. +# Test code style +To check the Python code style run: +```sh +pip install black +black --check **/**.py +# Fix by using +black **/**.py +``` diff --git a/mirte_robot/linetrace.py b/mirte_robot/linetrace.py index 2315fec..cc8776f 100644 --- a/mirte_robot/linetrace.py +++ b/mirte_robot/linetrace.py @@ -1,5 +1,5 @@ #!/usr/bin/env python -#TODO: for debugging purposes we could *also* listen to keyboard events +# TODO: for debugging purposes we could *also* listen to keyboard events import sys import os @@ -14,44 +14,46 @@ from mirte_robot import robot # Global shared memory objects (TODO: check if we need shared memory, why is server working?) -stepper = multiprocessing.Value('b', True) -do_step = multiprocessing.Value('b', False) -running = multiprocessing.Value('b', False) +stepper = multiprocessing.Value("b", True) +do_step = multiprocessing.Value("b", False) +running = multiprocessing.Value("b", False) + # the stop be stopped when: # 1) the code finishes # 2) the user stopped the process # 3) the websocket connection is closed def stop_mirte(): - global running - if running.value: + global running + if running.value: process.terminate() - running.value = False + running.value = False + def load_mirte_module(stepper, do_step, running): def trace_lines(frame, event, arg): - global do_step - if event != 'line': - return - # Only return line number to websocket when in step mode - if stepper.value: - server.send_message_to_all(str(frame.f_lineno)) - while stepper.value and not do_step.value: - time.sleep(.01) - do_step.value = False + global do_step + if event != "line": + return + # Only return line number to websocket when in step mode + if stepper.value: + server.send_message_to_all(str(frame.f_lineno)) + while stepper.value and not do_step.value: + time.sleep(0.01) + do_step.value = False def traceit(frame, event, arg): - co = frame.f_code - filename = co.co_filename - if not filename.endswith('mirte.py'): - return - return trace_lines + co = frame.f_code + filename = co.co_filename + if not filename.endswith("mirte.py"): + return + return trace_lines # Send the PID to the web interface and give it some time to call strace on this process # to see the output of this script server.send_message_to_all("pid:" + str(os.getpid())) - time.sleep(0.2) #TODO: let client send signal when strace is started + time.sleep(0.2) # TODO: let client send signal when strace is started sys.settrace(traceit) # rospy.init_node() for some reason needs to be called from __main__ when importing in the regular way. @@ -59,48 +61,55 @@ def traceit(frame, event, arg): # https://answers.ros.org/question/266612/rospy-init_node-inside-imported-file test = SourceFileLoader("mirte", "/home/mirte/workdir/mirte.py").load_module() - # Stop the motors. The atexit call in robot.py does not work when running from a subprocess: + # Stop the motors. The atexit call in robot.py does not work when running from a subprocess: # https://stackoverflow.com/questions/34506638/how-to-register-atexit-function-in-pythons-multiprocessing-subprocess # TODO: this assumes we have the robot initlized under variable 'mirte'. As soon as we let them create their own python, # this might not work anymore. - if hasattr(test, 'mirte'): - test.mirte.stop() + if hasattr(test, "mirte"): + test.mirte.stop() # Sending the linetrace 0 to the client server.send_message_to_all("0") running.value = False stop_mirte() -process = multiprocessing.Process(target = load_mirte_module, args=(stepper, do_step)) + +process = multiprocessing.Process(target=load_mirte_module, args=(stepper, do_step)) + def start_mirte(): global process # process should already have been killed after stop, or disconnect # but, just in case make sure to stop this if process.is_alive(): - process.terminate() - process = multiprocessing.Process(target = load_mirte_module, args=(stepper, do_step, running)) + process.terminate() + process = multiprocessing.Process( + target=load_mirte_module, args=(stepper, do_step, running) + ) process.start() + def client_left(client, server): stop_mirte() + def message_received(client, server, message): - global stepper, do_step, running - - if message == "b": #break (pause) - stepper.value = True - if message == "c": #continue (play) - stepper.value = False - if not running.value: - start_mirte() - running.value = True - if message == "s": #step (step) - do_step.value = True - if message == "e": #exit (stop) - stepper.value = True - do_step.value = False - stop_mirte() + global stepper, do_step, running + + if message == "b": # break (pause) + stepper.value = True + if message == "c": # continue (play) + stepper.value = False + if not running.value: + start_mirte() + running.value = True + if message == "s": # step (step) + do_step.value = True + if message == "e": # exit (stop) + stepper.value = True + do_step.value = False + stop_mirte() + server = WebsocketServer(host="0.0.0.0", port=8001, loglevel=logging.CRITICAL) server.set_fn_message_received(message_received) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index 7f274cc..cce652c 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -51,6 +51,7 @@ mirte = {} + class Robot: """Robot API @@ -69,6 +70,7 @@ class Robot: # function. Therefor the sphynx documentation is not able to process # this class anymore. _instance = None + def __new__(cls, *args, **kwargs): if cls._instance is None: cls._instance = super(Robot, cls).__new__(cls) @@ -84,24 +86,24 @@ def __init__( if getattr(self, "_initialized", False): return -# Parameters: -# machine_namespace (Optional[str], optional): The Namespace from '/' to the ROS namespace for the specific Mirte. Defaults to "/{HOSTNAME}". (This only has to be changed when running the Robot API from a different machine directly. It is configured correctly for the Web interface) -# hardware_namespace (str, optional): The namespace for the hardware peripherals. Defaults to "io". - - self._machine_namespace = "" #( -# machine_namespace -# if machine_namespace and validate_namespace(machine_namespace) -# else "/" + platform.node().replace("-", "_").lower() -# ) - self._hardware_namespace = "/io" #( -# hardware_namespace -# if validate_namespace( -# hardware_namespace -# if hardware_namespace.startswith("/") -# else (self._machine_namespace + "/" + hardware_namespace) -# ) -# else "io" -# ) + # Parameters: + # machine_namespace (Optional[str], optional): The Namespace from '/' to the ROS namespace for the specific Mirte. Defaults to "/{HOSTNAME}". (This only has to be changed when running the Robot API from a different machine directly. It is configured correctly for the Web interface) + # hardware_namespace (str, optional): The namespace for the hardware peripherals. Defaults to "io". + + self._machine_namespace = "" # ( + # machine_namespace + # if machine_namespace and validate_namespace(machine_namespace) + # else "/" + platform.node().replace("-", "_").lower() + # ) + self._hardware_namespace = "/io" # ( + # hardware_namespace + # if validate_namespace( + # hardware_namespace + # if hardware_namespace.startswith("/") + # else (self._machine_namespace + "/" + hardware_namespace) + # ) + # else "io" + # ) ROS_DISTRO = os.getenv("ROS_DISTRO") @@ -415,7 +417,6 @@ def finalize(node: rclpy.node.Node, motors: dict[str, rclpy.client.Client]): self.color_services[sensor]["RGBW"].wait_for_service() self.color_services[sensor]["HSL"].wait_for_service() - self._node.destroy_client(list_parameters) self._get_digital_pin_value_service = self._node.create_client( @@ -444,14 +445,14 @@ def _call_service( ) -> rclpy.client.SrvTypeResponse: with self._lock: - future_response = client.call_async(request) - while not future_response.done() and not self._stopping: - rclpy.spin_once(self._node, timeout_sec=0.1) + future_response = client.call_async(request) + while not future_response.done() and not self._stopping: + rclpy.spin_once(self._node, timeout_sec=0.1) if self._stopping: - with self._lock: - self._stopping = False - self._at_exit() + with self._lock: + self._stopping = False + self._at_exit() return future_response.result() @@ -863,13 +864,11 @@ def stop(self) -> None: def getROSNode(self): return self._node - def _signal_handler(self, sig, frame): self._stopping = True - if (not self._lock.locked()): - self._at_exit() - + if not self._lock.locked(): + self._at_exit() def _at_exit(self) -> None: self.stop() From 8f48cb91288b5845470e0b9426e30e2962519817 Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Mon, 19 May 2025 10:36:10 +0200 Subject: [PATCH 27/34] Also fixing style in setup --- README.md | 4 ++-- setup.py | 8 +++++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 26f08e9..fbbe35c 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ read the MIRTE documentation. To check the Python code style run: ```sh pip install black -black --check **/**.py +black --check . # Fix by using -black **/**.py +black . ``` diff --git a/setup.py b/setup.py index 0e86ecf..f2dea2a 100644 --- a/setup.py +++ b/setup.py @@ -5,7 +5,9 @@ setuptools.setup( name="mirte_robot", - install_requires=["websocket_server"], # TODO: Require mirte_msgs? rcl_interfaces? controller_manager_msgs? + install_requires=[ + "websocket_server" + ], # TODO: Require mirte_msgs? rcl_interfaces? controller_manager_msgs? # Also requires "rclpy" version="0.1.0", author="Martin Klomp", @@ -14,12 +16,12 @@ long_description=long_description, long_description_content_type="text/markdown", url="https://github.com/mirte-robot/mirte-python", - packages=['mirte_robot'], + packages=["mirte_robot"], license="Apache License 2.0", classifiers=[ "Programming Language :: Python :: 3", "License :: OSI Approved :: Apache Software License", "Operating System :: OS Independent", ], - python_requires='>=3.6', + python_requires=">=3.6", ) From f8b8149f789276473889bc1ae08d0c3303e4846a Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Wed, 28 May 2025 16:10:31 +0200 Subject: [PATCH 28/34] Added TU Delft open source statement --- LICENSE | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/LICENSE b/LICENSE index 307b1ab..d9aea46 100644 --- a/LICENSE +++ b/LICENSE @@ -186,7 +186,7 @@ same "printed page" as the copyright notice for easier identification within third-party archives. - Copyright 2025, TU Delft + Copyright 2025, MIRTE team Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. @@ -199,3 +199,8 @@ 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. + + Technische Universiteit Delft hereby disclaims all copyright interest + in the software of "MIRTE robot" (en educational robot) written by the + Author(s). + prof. dr. ir. Fred van Keulen, Dean of Mechanical Engineering From 25e0619f32a84949bdedfae52716d4313fcb23c9 Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Wed, 28 May 2025 16:22:39 +0200 Subject: [PATCH 29/34] Added links to readme --- README.md | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index fbbe35c..613545c 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,16 @@ # MIRTE Python API -This package provides the API for the MIRTE Robot. Please -read the MIRTE documentation. +This package provides the API for the [MIRTE robot](https://mirte.org). +Please read the [MIRTE documentation](https://docs.mirte.org/develop/doc/api/mirte_python_api.html) +for further documentation of this API. # Test code style -To check the Python code style run: +To cnotribute to this repository the code needs to pass the python +[style check](https://github.com/mirte-robot/mirte-python/blob/develop/.github/workflows/python-check.yml) + +To check this locally before you commit/push: ```sh pip install black black --check . -# Fix by using +# Fix by using: black . ``` From bef050c37e31927d54d82eb2c43009fcf72bd349 Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Fri, 30 May 2025 19:36:24 +0200 Subject: [PATCH 30/34] Modified README --- LICENSE | 2 +- README.md | 17 +++++++++++++---- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/LICENSE b/LICENSE index d9aea46..ac7b420 100644 --- a/LICENSE +++ b/LICENSE @@ -201,6 +201,6 @@ limitations under the License. Technische Universiteit Delft hereby disclaims all copyright interest - in the software of "MIRTE robot" (en educational robot) written by the + in the software of "MIRTE robot" (an educational robot) written by the Author(s). prof. dr. ir. Fred van Keulen, Dean of Mechanical Engineering diff --git a/README.md b/README.md index 613545c..2ee63fa 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,18 @@ -# MIRTE Python API -This package provides the API for the [MIRTE robot](https://mirte.org). +# mirte-python-api + +This package provides the python API for the [MIRTE robot](https://mirte.org). Please read the [MIRTE documentation](https://docs.mirte.org/develop/doc/api/mirte_python_api.html) for further documentation of this API. -# Test code style -To cnotribute to this repository the code needs to pass the python +## Install + +```sh +pip install . +``` + +## Checks + +To contribute to this repository the code needs to pass the python [style check](https://github.com/mirte-robot/mirte-python/blob/develop/.github/workflows/python-check.yml) To check this locally before you commit/push: @@ -14,3 +22,4 @@ black --check . # Fix by using: black . ``` + From a37c79941e2cd44c0e98ed9e9c84245f7576f261 Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Fri, 30 May 2025 19:37:32 +0200 Subject: [PATCH 31/34] Added OSS statement to README --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 2ee63fa..2730d27 100644 --- a/README.md +++ b/README.md @@ -23,3 +23,7 @@ black --check . black . ``` +## License + +This work is licensed under a Apache-2.0 OSS license. + From 695b9d489baecef725cb0805b112c135b990b7e0 Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Tue, 17 Jun 2025 13:52:02 +0200 Subject: [PATCH 32/34] Updated warnings for documentation --- mirte_robot/robot.py | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/mirte_robot/robot.py b/mirte_robot/robot.py index cce652c..d346de4 100644 --- a/mirte_robot/robot.py +++ b/mirte_robot/robot.py @@ -464,7 +464,7 @@ def _check_available(self, services: dict[str] | None, id: str) -> bool: return id in services def getTimestamp(self) -> float: - """Gets the elapsed time in seconds since the initialization fo the Robot. + """Gets the elapsed time in seconds since the initialization of the Robot. Returns: float: Time in seconds since the initialization of the Robot. Fractions of a second may be present if the system clock provides them. @@ -496,7 +496,7 @@ def getDistance(self, sensor: str) -> float: float: Range in meters measured by the HC-SR04 sensor. (The distance gets clamped to minimum and maximum range of the HC-SR04 sensor) Warning: - A maximum of 6 distance sensors is supported. + Depeding on the MCU used, there might be a mamimum of the number of distance sensors one can use. """ value = self._call_service(self.distance_services[sensor], GetRange.Request()) @@ -775,15 +775,14 @@ def setServoAngle(self, servo: str, angle: float) -> bool: bool: True if set successfully. Warning: - The servo uses the Servo library from Arduino (through Telemetrix). This also - means that, when a servo is used and the library is enabled, the last timer on - the MCU will be used for timing of the servos. This timer therefore can not be - used for PWM anymore. For Arduino Nano/Uno this means pins D9 and D10 will not - have PWM anymore. For the SMT32 this means pins A1, A2, A3, A15, B3, B10, and B11 - will not have PWM anymore. + Servos use PWM signals, which needs MCU timers. Depending on the MCU + and the servo implementation, it might be the case that enabling a servo + can interfere with (or use other) timers. This can lead to PWM signals not + functioning on some pins that used to have PWM (due to used timers). Warning: - A maximum of 12 servos is supported. + Depeding on the MCU used, there might be a mamimum of the number of servos + one can use. """ value = self._call_service( self.servo_services[servo], From 3cb21f98da129301a61a03ee4bc7c56745cad8b2 Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Mon, 7 Jul 2025 12:37:15 +0200 Subject: [PATCH 33/34] Modified TUD OSS waiver without changing the original Apache2 LICENCE --- NOTICE | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 NOTICE diff --git a/NOTICE b/NOTICE new file mode 100644 index 0000000..0090a31 --- /dev/null +++ b/NOTICE @@ -0,0 +1,7 @@ +This software is licensed under the Apache License, Version 2.0. +See the LICENSE file for details. + +Technische Universiteit Delft hereby disclaims all copyright interest +in the software of "MIRTE robot" (an educational robot) written by the +Author(s). +prof. dr. ir. Fred van Keulen, Dean of Mechanical Engineering From ef6ccd93515fa15c797888e9719753c6e28c1473 Mon Sep 17 00:00:00 2001 From: Martin Klomp Date: Mon, 7 Jul 2025 12:51:58 +0200 Subject: [PATCH 34/34] Added modified LICENCE --- LICENSE | 5 ----- 1 file changed, 5 deletions(-) diff --git a/LICENSE b/LICENSE index ac7b420..dee5c37 100644 --- a/LICENSE +++ b/LICENSE @@ -199,8 +199,3 @@ 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. - - Technische Universiteit Delft hereby disclaims all copyright interest - in the software of "MIRTE robot" (an educational robot) written by the - Author(s). - prof. dr. ir. Fred van Keulen, Dean of Mechanical Engineering