From 18a2d08ba80578f45eb9774ac3ca439534b693dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bj=C3=B6rn=20Ellensohn?= Date: Fri, 18 Aug 2023 10:31:49 +0200 Subject: [PATCH] fixes --- .../__pycache__/__init__.cpython-310.pyc | Bin 199 -> 0 bytes .../rmp220_middleware.cpython-310.pyc | Bin 3842 -> 0 bytes rmp220_middleware/rmp220_middleware copy.py | 96 ------------------ rmp220_middleware/rmp220_middleware.py | 2 +- 4 files changed, 1 insertion(+), 97 deletions(-) delete mode 100644 rmp220_middleware/__pycache__/__init__.cpython-310.pyc delete mode 100644 rmp220_middleware/__pycache__/rmp220_middleware.cpython-310.pyc delete mode 100644 rmp220_middleware/rmp220_middleware copy.py diff --git a/rmp220_middleware/__pycache__/__init__.cpython-310.pyc b/rmp220_middleware/__pycache__/__init__.cpython-310.pyc deleted file mode 100644 index e09dd6603f25d8e020c26b8aaba47c5af4e18eda..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 199 zcmd1j<>g`kf+Zr?Qb6=$5P=LBfgA@QE@lA|DGb33nv8xc8Hzx{2;x_kenx(7s(w;d zeo>yjOMY@`Zfaghv3^m0aeP5hepYHSh?84jWMmMZo0*c5lUklw1XNy{nUjJ=98+a{ gd}dx|NqoFsLFFwDo80`A(wtN~kQ<7bfCLKz08$w=!vFvP diff --git a/rmp220_middleware/__pycache__/rmp220_middleware.cpython-310.pyc b/rmp220_middleware/__pycache__/rmp220_middleware.cpython-310.pyc deleted file mode 100644 index 2ba5da2ca496dd6d1e476f017fdb56494fd0bb59..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3842 zcmaJ^&2JmW6`$E%l1p+)Q6E-qrGaT6t+QxKwQk_LMi8sE95?;o!Ezgvhb}12np$eP zE6*$~i%?E7kW2FiwCF(sdZ=%`7U+M`+g@`BbSscs(Zv0|*(F7aPM4TBGw;pkd++z& zFs{`q4BuZ1sd#*au`j4I`&sC0p{T!s2qt*QI#e4W9~vElQ`-#9q1CZ;+X@TAVyCFv zg|IZVJGO2Y!}74wspxhotPY)yquV^Jb!vLudZ(`IrOwhLCTvlD$wb+&oS2;k`V~<{ zzv>(4FN-DNh}uiuS@BmzeczCe{go4j_4tl1ZsQ!MWo?(IRy!6x)%W8#L|u4t7^nmd z^Ox}v@gGK`A?o$VvD)#I2YX$m0`)NJ?uPz@q3F>%vmZxi3q`Gg_^iVP>llJ}Oks2^ zVeT;3N((zlH}NG6Jtxw33#sjSQFrKjURv?IVJt==wH?oUHtL4C#NPgR=jZo-{ct-i zJ#6d7D<+q)fF}P@n|txl-`pLcyVeQFzIqL|OvQ!cnLUfNKQDXfVhZ<8+B2XMFn+9RE2}u{)}0yQ-&gIX+taWH{IS|5cy=t9;tKkQJYRhJF9xOJzC-uB;{(TSrUQL3bkCi;-_r@RW`0{zs^P5qeG`r8cK}hcZUh}XUBtg_~ zK8k%Q)v=whLkyO9>u=)ji*gCGhL&(lsMYd8@^bnUd1pD1ng zq)&P{8106E+ViEPAe2@o_Xj^+&Bv(GuIkC)APM3K6LSsqx?#B6?d@O8!8lo*Tt?TMySRrE<5{eyZJ;do0l$-6A*7CW%FaRKBiMF{iuX!(%r47=k z2X#X2TH|S@4>N^vzmILLAbJ|VLQGPA_!M>7!#uHR z*7zE4aNDwtx>@J=y})q5ZbKy=1F1(1%p2e%)Xr`E9s z72LJ}mo5rp>b7S4$#_+VOLCO_*6@l~rA!f}$NsYsw3Tj9oLclr(xP;+Yg!?zcwKcA z_2du0kaU(;hHR10l>W1eP$)?gX+3kriIcw%-d7Z^Y+mFJU*Y5H3ksN@@vUl*KgKey z+5Tv}wjC&7Vg7FN7R2&4k!f_!j+>tZ;i_o|T2D)7g0zMn2K= z3i-U>(Fx}EE9Udcv>%1BNpVZ<-2d)2^IX<^7a3P2L2utZCydB4NN%HV5=YIO z2nsT}>7G-BAK%d0yW5;3$mYA#VSb3jchhw%a)TBn2hd?fHmOx7GW8~T2i>%^tG7^+ ztm94`Y^eH|tSN6mzJ;P_<#lAfI?Bde4FG;UetW^Tvq5IgumrRT*wkOpnK9B&$*>cs z0eqCV_@Fo_2{QZl$P|Ne)=nxX?9&M}H5enxTNkq_%Tac_%*ocX_L+K+uZOn$#WC$x zCF9Y`leSxE%T=5)&l}UmJ6rvq=vlhXaTg`Bh#t>fAvLq;l1$aL34e^KA0;mqML++2qpGK9QGgmf&wPvnP0=3|z8 z2RwnIU$l-{(f(cGby6&%sP*~{adbXE0(uCb(UNda8BF~VBw+(MwBVoXhajNB z$D894@|v0Du}@f3wpd~vZE(ab&VJ9L#v%KJ9dnAKh@AYiNHu0?>ynlJzx3N?9wNZ# zzad@OSFW8FWG_58qD&to`XWg72SJoB|H?nwjk{8O9O0jX933QDbEzYcJ0%_|^cYNS zwKqycd>A2WeS~|a7O^iGG_{7^Ad;jDGA+TxLgaEljn~F&JQC10{%adl+uzu{!X?3q z-exq@^IYC`*W@GORrGmVdK*%6>Zn8zI8}=fO1Mdv`#%1cB=YEjuODnn%8wMDI@8hm z0cQF5!zC%Irx?&b?B>&ye4f1+ig;bU2$Hm@f_~HuQ&SzOEUc5^^-(i-yYOS((r*I$ igY3W6hlFa?1hEYY6_cNB+VJm&(Xc9iu$vWl_x}LMdZ#P^ diff --git a/rmp220_middleware/rmp220_middleware copy.py b/rmp220_middleware/rmp220_middleware copy.py deleted file mode 100644 index 7f0d883..0000000 --- a/rmp220_middleware/rmp220_middleware copy.py +++ /dev/null @@ -1,96 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from enum import Enum -from rclpy.node import Node -from std_msgs.msg import Bool -from geometry_msgs.msg import Twist -#from sensor_msgs.msg import Joy -from segway_msgs.srv import RosSetChassisEnableCmd - -class State(Enum): - DISABLED = 0 - ENABLED = 1 - -class StateMachineNode(Node): - def __init__(self): - super().__init__('state_machine_node') - - # Initialize state and other variables - self.state = State.DISABLED - self.timeout = 2.0 # Timeout in seconds - - # Create publishers, subscribers, timers, and service clients here - self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel_out', 10) - self.cmd_vel_sub = self.create_subscription(Twist, '/cmd_vel_mux', self.cmd_vel_callback, 10) - #self.joy_sub = self.create_subscription(Joy, '/joy', self.joy_callback, 10) - self.timer = self.create_timer(0.1, self.timer_callback) - - # Create service clients for chassis enable and disable - self.chassis_enable_client = self.create_client(RosSetChassisEnableCmd, 'set_chassis_enable') - while not self.chassis_enable_client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('Service not available, waiting for chassis enable service...') - self.chassis_disable_client = self.create_client(RosSetChassisEnableCmd, 'set_chassis_enable') - while not self.chassis_disable_client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('Service not available, waiting for chassis disable service...') - - def joy_callback(self, msg): - # Implement logic to detect joystick button presses (start/select) and update state - # ... - if msg.buttons[7] == 1: # Joystick button 'start' - self.state = State.ENABLED - self.get_logger().info("State: ENABLED (Button 'start')") - self.enable_chassis() - if msg.buttons[6] == 1: # Joystick button 'select' - self.state = State.DISABLED - self.get_logger().info("State: DISABLED (Button 'select')") - self.disable_chassis() - - def enable_chassis(self): - req = RosSetChassisEnableCmd.Request() - req.ros_set_chassis_enable_cmd = True - self.chassis_enable_client.call_async(req) - - def disable_chassis(self): - req = RosSetChassisEnableCmd.Request() - req.ros_set_chassis_enable_cmd = False - self.chassis_disable_client.call_async(req) - - def cmd_vel_callback(self, msg): - # Update state to ENABLED upon receiving a command on /cmd_vel_mux - # ... - if self.state == State.ENABLED: - self.cmd_vel_pub.publish(msg) - self.timeout = 2.0 # Reset timeout when receiving commands - - def timer_callback(self): - # Republish the cmd_vel_mux command to cmd_vel_out topic - # ... - - # Reset the timeout counter - # ... - - # Check if the timeout has been exceeded, and if so, switch to DISABLED - # ... - - if self.state == State.ENABLED: - if self.timeout <= 0: - self.state = State.DISABLED - self.get_logger().info("State: DISABLED (Timeout)") - self.disable_chassis() - else: - self.timeout -= 0.1 - -def main(args=None): - rclpy.init(args=args) - node = StateMachineNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/rmp220_middleware/rmp220_middleware.py b/rmp220_middleware/rmp220_middleware.py index 595c6ce..d716b91 100644 --- a/rmp220_middleware/rmp220_middleware.py +++ b/rmp220_middleware/rmp220_middleware.py @@ -81,7 +81,7 @@ class StateMachineNode(Node): else: self.timeout -= 0.01 self.cmd_vel_pub.publish(self.latest_cmd_vel) - if self.state == State.DISABLED and (abs(self.latest_cmd_vel.linear) > 0.1 or abs(self.latest_cmd_vel.angular > 0.1)): # This is a hack to enable the chassis when receiving commands e.g. from Nav2 + if self.state == State.DISABLED and (abs(self.latest_cmd_vel.linear.x) > 0.1 or abs(self.latest_cmd_vel.angular.z > 0.1)): # This is a hack to enable the chassis when receiving commands e.g. from Nav2 self.state = State.ENABLED self.get_logger().info("State: ENABLED (cmd_vel)") self.enable_chassis()