-
Notifications
You must be signed in to change notification settings - Fork 2
Expand file tree
/
Copy pathremote_control.py
More file actions
164 lines (142 loc) ยท 5.64 KB
/
remote_control.py
File metadata and controls
164 lines (142 loc) ยท 5.64 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
#!/usr/bin/env python3
"""
Remote Motor Control Script
Send motor commands over network to ROS2 system
"""
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import sys
import time
class RemoteMotorController(Node):
def __init__(self):
super().__init__('remote_motor_controller')
self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)
self.get_logger().info('๐ฎ Remote Motor Controller Ready!')
def send_command(self, linear_x=0.0, angular_z=0.0):
"""Send velocity command to motors"""
msg = Twist()
msg.linear.x = linear_x
msg.linear.y = 0.0
msg.linear.z = 0.0
msg.angular.x = 0.0
msg.angular.y = 0.0
msg.angular.z = angular_z
self.cmd_pub.publish(msg)
self.get_logger().info(f'๐ค Sent: Linear={linear_x}, Angular={angular_z}')
def keyboard_control():
"""Interactive keyboard control mode"""
print("\n๐ฎ Remote Lawnmower Control")
print("Setting up ROS2 network...")
# Configure ROS2 for network communication
import os
os.environ['ROS_DOMAIN_ID'] = '42'
os.environ['RMW_IMPLEMENTATION'] = 'rmw_fastrtps_cpp'
os.environ['ROS_LOCALHOST_ONLY'] = '0'
os.environ['ROS_AUTOMATIC_DISCOVERY_RANGE'] = 'SUBNET'
try:
rclpy.init()
controller = RemoteMotorController()
print("โ
Connected to robot network!")
# Give time for discovery
time.sleep(2)
except Exception as e:
print(f"โ ROS2 initialization failed: {e}")
print("โ ๏ธ Make sure robot is running and on same network")
return
print("Controls:")
print(" W - Forward")
print(" S - Backward")
print(" A - Turn Left")
print(" D - Turn Right")
print(" Q - Quit")
print(" SPACE - Stop")
print("\nPress keys and ENTER:")
try:
while True:
key = input(">>> ").strip().lower()
if key == 'w':
print("๐ผ Sending Forward Command...")
controller.send_command(0.3, 0.0) # Forward
print("โ
Forward command sent!")
elif key == 's':
print("๐ฝ Sending Backward Command...")
controller.send_command(-0.3, 0.0) # Backward
print("โ
Backward command sent!")
elif key == 'a':
print("โ๏ธ Sending Turn Left Command...")
controller.send_command(0.0, 1.5) # Increased angular velocity
print("โ
Turn left command sent!")
elif key == 'd':
print("โถ๏ธ Sending Turn Right Command...")
controller.send_command(0.0, -1.5) # Increased angular velocity
print("โ
Turn right command sent!")
elif key == ' ' or key == 'stop':
print("โน๏ธ Sending Stop Command...")
controller.send_command(0.0, 0.0) # Stop
print("โ
Stop command sent!")
elif key == 'q' or key == 'quit':
print("๐ Stopping and quitting...")
controller.send_command(0.0, 0.0) # Stop before quit
print("๐ Goodbye!")
break
elif key == 'status':
print("๐ Checking system status...")
# Add status check here if needed
else:
print("โ Unknown command. Use W/A/S/D/SPACE/Q")
except KeyboardInterrupt:
print("\n๐ Stopping motors...")
controller.send_command(0.0, 0.0) # Stop on Ctrl+C
print("\n๐ Goodbye!")
try:
controller.destroy_node()
rclpy.shutdown()
except:
pass
def main():
# Check if keyboard mode requested
if len(sys.argv) == 2 and sys.argv[1] == "keyboard":
keyboard_control()
return
elif len(sys.argv) == 2 and sys.argv[1] == "test":
# Quick test mode - just send one forward command
print("๐ Quick Test Mode - Sending forward command...")
try:
rclpy.init()
controller = RemoteMotorController()
controller.send_command(0.3, 0.0) # Forward
print("โ
Forward command sent!")
time.sleep(1)
controller.send_command(0.0, 0.0) # Stop
print("โน๏ธ Stop command sent!")
controller.destroy_node()
rclpy.shutdown()
except Exception as e:
print(f"โ Test failed: {e}")
return
if len(sys.argv) < 3:
print("Usage:")
print(" python3 remote_control.py keyboard # Interactive keyboard mode")
print(" python3 remote_control.py test # Quick test mode")
print(" python3 remote_control.py <linear_x> <angular_z> # Direct command")
print("\nDirect command examples:")
print(" python3 remote_control.py 0.3 0.0 # Forward")
print(" python3 remote_control.py -0.3 0.0 # Backward")
print(" python3 remote_control.py 0.0 0.5 # Turn left")
print(" python3 remote_control.py 0.0 -0.5 # Turn right")
print(" python3 remote_control.py 0.0 0.0 # Stop")
sys.exit(1)
linear_x = float(sys.argv[1])
angular_z = float(sys.argv[2])
rclpy.init()
controller = RemoteMotorController()
# Send command
controller.send_command(linear_x, angular_z)
# Keep alive briefly to ensure message is sent
time.sleep(0.5)
controller.destroy_node()
rclpy.shutdown()
print(f"โ
Command sent: Linear={linear_x} m/s, Angular={angular_z} rad/s")
if __name__ == '__main__':
main()