The site is read-only. Please transition to use Robotics Stack Exchange
| ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
 | 1 | initial version |
So, as there has been no answer - I am posting the solution I found myself...
It seems the
We wrote a code to teleop_twist_keyboard doesn't give the quadrotor linear z velocity to go upward. So, if you start the quadrotor at a height (let's say 1000 m) ... while it falls down you can move left/right and so on.
We wrote our own module to test it out which works. Here's the code :
#!/usr/bin/env python
import rospy
import time
from geometry_msgs.msg import Twist
import pygame, sys
class KeyboardController:
def __init__(self, type="cartesian"):
self.type = type
self._gui_init()
self._rospy_init()
def _gui_init(self):
pygame.init()
self.screen = pygame.display.set_mode([300, 300])
self.clock = pygame.time.Clock()
background = pygame.Surface(self.screen.get_size())
background = background.convert()
background.fill((250, 250, 250))
font = pygame.font.Font(None, 36)
text = font.render("Keyboard controller", 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
background.blit(text, textpos)
font = pygame.font.Font(None, 30)
text = font.render("Not connected", 1, (200, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = background.get_rect().centery
background.blit(text, textpos)
self.screen.blit(background, (0, 0))
pygame.display.flip()
def _rospy_init(self, rate=10):
self.publisher = rospy.Publisher('cmd_vel', Twist, queue_size=10)
rospy.init_node('controller', anonymous=True)
self.rate = rospy.Rate(rate) # in Hz
self.msg = Twist()
def run(self):
while not rospy.is_shutdown():
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
sys.exit()
self._event(pygame.key.get_pressed())
self._draw()
rospy.loginfo(self.msg)
self.publisher.publish(self.msg)
self._sleep()
def _event(self, key_events):
if self.type == "cartesian":
self._cartesian_event(key_events)
else:
print("Unknown keyboard controlle rtype:", self.type)
def _cartesian_event(self, key_events):
if key_events[pygame.K_UP]:
self.msg.linear.z = 1;
elif key_events[pygame.K_DOWN]:
self.msg.linear.z = -1;
if key_events[pygame.K_w]:
self.msg.linear.y = 1;
elif key_events[pygame.K_s]:
self.msg.linear.y = -1;
if key_events[pygame.K_d]:
self.msg.linear.x = 1;
elif key_events[pygame.K_a]:
self.msg.linear.x = -1;
def _draw(self):
background = pygame.Surface(self.screen.get_size())
background = background.convert()
background.fill((250, 250, 250))
centery = 20
font = pygame.font.Font(None, 36)
text = font.render("Keyboard controller", 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
background.blit(text, textpos)
centery += 20 + 15 + 30
font = pygame.font.Font(None, 30)
text = font.render("Linear vel", 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = centery
background.blit(text, textpos)
centery += 15 + 10
font = pygame.font.Font(None, 20)
text = font.render("X : " + str(self.msg.linear.x), 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = centery
background.blit(text, textpos)
centery += 10 + 10
text = font.render("Y : " + str(self.msg.linear.y), 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = centery
background.blit(text, textpos)
centery += 10 + 10
text = font.render("Z : " + str(self.msg.linear.z), 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = centery
background.blit(text, textpos)
centery += 10 + 15 + 30
font = pygame.font.Font(None, 30)
text = font.render("Angular vel", 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = centery
background.blit(text, textpos)
centery += 15 + 10
font = pygame.font.Font(None, 20)
text = font.render("W1 : " + str(self.msg.angular.x), 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = centery
background.blit(text, textpos)
centery += 10 + 10
text = font.render("W2 : " + str(self.msg.angular.y), 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = centery
background.blit(text, textpos)
centery += 10 + 10
text = font.render("W3 : " + str(self.msg.angular.z), 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = centery
background.blit(text, textpos)
self.screen.blit(background, (0, 0))
pygame.display.flip()
def _sleep(self):
self.rate.sleep()
if __name__ == '__main__':
try:
keyboard = KeyboardController(type="cartesian")
keyboard.run()
except rospy.ROSInterruptException:
print("Closed the program due to ROS Interrupt exception")
except KeyboardInterrupt:
print("Closed the program due to Keyboard Interrupt exception")
| | 2 | No.2 Revision |
So, as there has been no answer - I am posting the solution I found myself...
It seems the
We wrote a code to teleop_twist_keyboard doesn't give the quadrotor linear z velocity to go upward. So, if you start the quadrotor at a height (let's say 1000 m) ... while it falls down you can move left/right and so on.
We wrote our own module to test it out which works. Here's the code :
#!/usr/bin/env python
import rospy
import Tkinter as tk
import threading
import thread
import time
from geometry_msgs.msg import Twist
import pygame, sys
class KeyboardController:
# Getch implementation, not used in Current function
try:
from msvcrt import getch # try to import Windows version
except ImportError:
def __init__(self, type="cartesian"):
self.type = type
self._gui_init()
self._rospy_init()
getch(): # define non-Windows version
import sys, tty, termios
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
print ch
return ch
char = None
def _gui_init(self):
pygame.init()
self.screen = pygame.display.set_mode([300, 300])
self.clock = pygame.time.Clock()
background = pygame.Surface(self.screen.get_size())
background = background.convert()
background.fill((250, 250, 250))
font = pygame.font.Font(None, 36)
text = font.render("Keyboard controller", 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
background.blit(text, textpos)
font = pygame.font.Font(None, 30)
text = font.render("Not connected", 1, (200, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = background.get_rect().centery
background.blit(text, textpos)
self.screen.blit(background, (0, 0))
pygame.display.flip()
keypress():
global char
char = getch()
def _rospy_init(self, rate=10):
self.publisher talker():
global clock
pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
rospy.init_node('controller', anonymous=True)
self.rate = rospy.Rate(rate) rate = rospy.Rate(10) # in Hz
self.msg = Twist()
def run(self):
10hz
while not rospy.is_shutdown():
msg = Twist()
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
sys.exit()
self._event(pygame.key.get_pressed())
self._draw()
rospy.loginfo(self.msg)
self.publisher.publish(self.msg)
self._sleep()
def _event(self, key_events):
array = pygame.key.get_pressed()
if self.type == "cartesian":
self._cartesian_event(key_events)
else:
print("Unknown keyboard controlle rtype:", self.type)
def _cartesian_event(self, key_events):
array[pygame.K_UP]:
msg.linear.z = 1;
elif array[pygame.K_DOWN]:
msg.linear.z = -1;
if key_events[pygame.K_UP]:
self.msg.linear.z array[pygame.K_w]:
msg.linear.y = 1;
elif key_events[pygame.K_DOWN]:
self.msg.linear.z array[pygame.K_s]:
msg.linear.y = -1;
if key_events[pygame.K_w]:
self.msg.linear.y array[pygame.K_d]:
msg.linear.x = 1;
elif key_events[pygame.K_s]:
self.msg.linear.y array[pygame.K_a]:
msg.linear.x = -1;
if key_events[pygame.K_d]:
self.msg.linear.x = 1;
elif key_events[pygame.K_a]:
self.msg.linear.x = -1;
def _draw(self):
background = pygame.Surface(self.screen.get_size())
background = background.convert()
background.fill((250, 250, 250))
centery = 20
font = pygame.font.Font(None, 36)
text = font.render("Keyboard controller", 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
background.blit(text, textpos)
centery += 20 + 15 + 30
font = pygame.font.Font(None, 30)
text = font.render("Linear vel", 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = centery
background.blit(text, textpos)
centery += 15 + 10
font = pygame.font.Font(None, 20)
text = font.render("X : " + str(self.msg.linear.x), 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = centery
background.blit(text, textpos)
centery += 10 + 10
text = font.render("Y : " + str(self.msg.linear.y), 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = centery
background.blit(text, textpos)
centery += 10 + 10
text = font.render("Z : " + str(self.msg.linear.z), 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = centery
background.blit(text, textpos)
centery += 10 + 15 + 30
font = pygame.font.Font(None, 30)
text = font.render("Angular vel", 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = centery
background.blit(text, textpos)
centery += 15 + 10
font = pygame.font.Font(None, 20)
text = font.render("W1 : " + str(self.msg.angular.x), 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = centery
background.blit(text, textpos)
centery += 10 + 10
text = font.render("W2 : " + str(self.msg.angular.y), 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = centery
background.blit(text, textpos)
centery += 10 + 10
text = font.render("W3 : " + str(self.msg.angular.z), 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = background.get_rect().centerx
textpos.centery = centery
background.blit(text, textpos)
self.screen.blit(background, (0, 0))
pygame.display.flip()
def _sleep(self):
self.rate.sleep()
rospy.loginfo(msg)
pub.publish(msg)
rate.sleep()
pygame.init()
size = [100, 100]
screen = pygame.display.set_mode(size)
clock = pygame.time.Clock()
if __name__ == '__main__':
# threading.Thread(target=tk_func())
try:
keyboard = KeyboardController(type="cartesian")
keyboard.run()
talker()
except rospy.ROSInterruptException:
print("Closed the program due to ROS Interrupt exception")
except KeyboardInterrupt:
print("Closed the program due to Keyboard Interrupt exception")
pass
| | 3 | No.3 Revision |
So, as there has been no answer - I am posting the solution I found myself...
It seems the
We wrote a code to teleop_twist_keyboard doesn't give the quadrotor linear z velocity to go upward. So, if you start the quadrotor at a height (let's say 1000 m) ... while it falls down you can move left/right and so on.
We wrote our own module to test it out which works. Here's the code :
#!/usr/bin/env python
import rospy
import Tkinter as tk
import threading
import thread
import time
from geometry_msgs.msg import Twist
import pygame, sys
# Getch implementation, not used in Current function
try:
from msvcrt import getch # try to import Windows version
except ImportError:
def getch(): # define non-Windows version
import sys, tty, termios
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
print ch
return ch
char = None
def keypress():
global char
char = getch()
def talker():
global clock
pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
rospy.init_node('controller', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
msg = Twist()
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
sys.exit()
array = pygame.key.get_pressed()
if array[pygame.K_UP]:
msg.linear.z = 1;
elif array[pygame.K_DOWN]:
msg.linear.z = -1;
if array[pygame.K_w]:
msg.linear.y = 1;
elif array[pygame.K_s]:
msg.linear.y = -1;
if array[pygame.K_d]:
msg.linear.x = 1;
elif array[pygame.K_a]:
msg.linear.x = -1;
rospy.loginfo(msg)
pub.publish(msg)
rate.sleep()
pygame.init()
size = [100, 100]
screen = pygame.display.set_mode(size)
clock = pygame.time.Clock()
if __name__ == '__main__':
# threading.Thread(target=tk_func())
try:
talker()
except rospy.ROSInterruptException:
pass
ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.