def publish(self): cmd = Joy() cmd.header.stamp = rospy.Time.now() cmd.axes = self.axes_data cmd.buttons = self.button_data if self.debug: print cmd self.pub.publish(cmd)
def talker():
pub = rospy.Publisher('joy', Joy, queue_size=20)
rospy.init_node('test', anonymous=True)
rate = rospy.Rate(100) # 10hz
bits=[]
#print axis,buttons
while not rospy.is_shutdown():
data = s.recvfrom(6144)
app_data = data[0]
app_data_addr = data[1]
app_data=app_data.strip("[]")
app_data=app_data.split(",")
app_data=map(int,app_data)
#print app_data
#app_data=[10,10,0,0,0,0,0,0,0,0]
recv = robot1.receiver(app_data)
app_axis_data,app_button_data=recv.sort_data()
axis,buttons=recv.joystick_data(app_axis_data,app_button_data)
joy=Joy()
joy.axes=axis
joy.buttons=buttons
#hello_str = "hello world %s" % rospy.get_time()
rospy.loginfo(app_data)
rospy.loginfo(buttons)
pub.publish(joy)
rate.sleep()
def compare_all(data):
global k
last_row = csv_data[k-frequency_offset]
diff = Quaternion()
diff.x = round(float(last_row[1])-convert_ppm(data.channels[3]),4)
diff.y = round(float(last_row[2])-convert_ppm(data.channels[1]),4)
diff.z = round(float(last_row[4])-convert_ppm(data.channels[0]),4)
diff.w = round(float(last_row[5])+convert_ppm(data.channels[2]),4)
testing_pub.publish(diff)
if (k < len(csv_data)):
row = csv_data[k]
#log for post processing
#joystick log
c = csv.writer(open("/home/jinahadam/catkin_ws/src/testing/gps_joy.csv", "ab"))
c.writerow([data.header.stamp.secs,row[1],row[2],row[4],row[5]])
#rc Inn log
c = csv.writer(open("/home/jinahadam/catkin_ws/src/testing/rc_in.csv", "ab"))
c.writerow([data.header.stamp.secs,convert_ppm(data.channels[3]),convert_ppm(data.channels[1]),convert_ppm(data.channels[0]),convert_ppm(data.channels[2])])
msg = Joy()
msg.axes = (float(row[1]),float(row[2]),float(row[3]),float(row[4]),float(row[5]),float(row[6]),float(row[7]))
msg.buttons = (0,0,0,0,0,0,0,0,0,0,0)
k = k + 1
pub.publish(msg)
else:
#k = 0
print("Joystick & RC In data logged with GPS Time")
exit()
def joy_callback(self, data):
joy = Joy()
joy.header.stamp = rospy.Time.now()
joy.axes = self.convert(data.axes)
joy.buttons = data.buttons
self.prev_input_axes = list(data.axes)
joy_pub = rospy.Publisher("/joy_relative/page_" + str(self.crt_page), Joy)
joy_pub.publish(joy)
def callback(data):
pub = rospy.Publisher('joy', Joy)
buttons = [0] * (trigger_button + 1)
buttons[trigger_button] = 1
joy_msg = Joy()
joy_msg.buttons = buttons
pub.publish(joy_msg)
def run(self):
"""Loop that obtains the latest wiimote state, publishes the joystick data, and sleeps.
The Joy.msg message types calls for just two fields: float32[] axes, and int32[] buttons.
"""
rospy.loginfo("Wiimote joystick publisher starting (topic wiijoy).")
self.threadName = "Joy topic Publisher"
try:
while not rospy.is_shutdown():
(canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData()
msg = Joy(header=None,
axes=[canonicalAccel[X], canonicalAccel[Y], canonicalAccel[Z]],
buttons=None)
# If a gyro is attached to the Wiimote, we add the
# gyro information:
if self.wiistate.motionPlusPresent:
msg.axes.extend([canonicalAngleRate[PHI], canonicalAngleRate[THETA], canonicalAngleRate[PSI]])
# Fill in the ROS message's buttons field (there *must* be
# a better way in python to declare an array of 11 zeroes...]
theButtons = [False,False,False,False,False,False,False,False,False,False,False]
theButtons[State.MSG_BTN_1] = self.wiistate.buttons[BTN_1]
theButtons[State.MSG_BTN_2] = self.wiistate.buttons[BTN_2]
theButtons[State.MSG_BTN_A] = self.wiistate.buttons[BTN_A]
theButtons[State.MSG_BTN_B] = self.wiistate.buttons[BTN_B]
theButtons[State.MSG_BTN_PLUS] = self.wiistate.buttons[BTN_PLUS]
theButtons[State.MSG_BTN_MINUS] = self.wiistate.buttons[BTN_MINUS]
theButtons[State.MSG_BTN_LEFT] = self.wiistate.buttons[BTN_LEFT]
theButtons[State.MSG_BTN_RIGHT] = self.wiistate.buttons[BTN_RIGHT]
theButtons[State.MSG_BTN_UP] = self.wiistate.buttons[BTN_UP]
theButtons[State.MSG_BTN_DOWN] = self.wiistate.buttons[BTN_DOWN]
theButtons[State.MSG_BTN_HOME] = self.wiistate.buttons[BTN_HOME]
msg.buttons = theButtons
measureTime = self.wiistate.time
timeSecs = int(measureTime)
timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
# Add the timestamp
msg.header.stamp.secs = timeSecs
msg.header.stamp.nsecs = timeNSecs
try:
self.pub.publish(msg)
except rospy.ROSException:
rospy.loginfo("Topic wiijoy closed. Shutting down Joy sender.")
exit(0)
#rospy.logdebug("Joystick state:")
#rospy.logdebug(" Joy buttons: " + str(theButtons) + "\n Joy accel: " + str(canonicalAccel) + "\n Joy angular rate: " + str(canonicalAngleRate))
rospy.sleep(self.sleepDuration)
except rospy.ROSInterruptException:
rospy.loginfo("Shutdown request. Shutting down Joy sender.")
exit(0)
def on_message(self, message_raw):
message = json.loads(message_raw)
if message['msg']=='lag':
self.send( json.dumps({
'start':message['start'],
}))
if message['msg']=='joy':
msg = Joy()
msg.header.stamp = rospy.Time.now()
msg.axes = message['axes']
msg.buttons = message['buttons']
joy_pub.publish(msg)
def send(self):
joy_msg = Joy()
joy_msg.header.stamp = rospy.Time.now()
buttons = (self.ui.button_0, self.ui.button_1, self.ui.button_2, self.ui.button_3, self.ui.button_4,
self.ui.button_5, self.ui.button_6, self.ui.button_7, self.ui.button_8, self.ui.button_9,
self.ui.button_10, self.ui.button_11)
joy_msg.buttons = [b.isChecked() for b in buttons]
joy_msg.axes = [mult*s.value()/100.0 for s, mult in
zip((self.ui.rollSlider, self.ui.pitchSlider, self.ui.yawSlider, self.ui.altSlider), (-1.0, 1.0, -1.0, 1.0))]
for label, value in zip((self.ui.rollLabel, self.ui.pitchLabel, self.ui.yawLabel, self.ui.altLabel), joy_msg.axes):
label.setText('%0.2f' % value)
self.pub.publish(joy_msg)
def run(self):
"""Loop that obtains the latest nunchuk state, publishes the joystick data, and sleeps.
The Joy.msg message types calls for just two fields: float32[] axes, and int32[] buttons.
"""
self.threadName = "nunchuk Joy topic Publisher"
try:
while not rospy.is_shutdown():
rospy.sleep(self.sleepDuration)
(canonicalAccel, scaledAcc, canonicalAngleRate) = self.obtainWiimoteData()
if not self.wiistate.nunchukPresent:
continue
if self.pub is None:
self.pub = rospy.Publisher('/wiimote/nunchuk', Joy)
rospy.loginfo("Wiimote Nunchuk joystick publisher starting (topic nunchuk).")
(joyx, joyy) = self.wiistate.nunchukStick
msg = Joy(header=None,
axes=[joyx, joyy,
scaledAcc[X], scaledAcc[Y], scaledAcc[Z]],
buttons=None)
theButtons = [False,False]
theButtons[State.MSG_BTN_Z] = self.wiistate.nunchukButtons[BTN_Z]
theButtons[State.MSG_BTN_C] = self.wiistate.nunchukButtons[BTN_C]
msg.buttons = theButtons
measureTime = self.wiistate.time
timeSecs = int(measureTime)
timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
msg.header.stamp.secs = timeSecs
msg.header.stamp.nsecs = timeNSecs
try:
self.pub.publish(msg)
except rospy.ROSException:
rospy.loginfo("Topic /wiimote/nunchuk closed. Shutting down Nun sender.")
exit(0)
#rospy.logdebug("nunchuk state:")
#rospy.logdebug(" nunchuk buttons: " + str(theButtons) + "\n Nuchuck axes: " + str(msg.axes) + "\n")
except rospy.ROSInterruptException:
rospy.loginfo("Shutdown request. Shutting down Nun sender.")
exit(0)
def run(self):
blub=Joy()
while(self.run):
data=ord(self.ser.read(1))
if data==255:
data=ord(self.ser.read(1))
if data==255:
data=ord(self.ser.read(1))
if data==255:
data=ord(self.ser.read(1))
if data==24:
data=self.ser.read(7)
axes = struct.unpack('BBBBBBB', data)
for x in axes:
blub.axes.append((x-128)/127.0)
print blub.axes
data=self.ser.read(2)
button = struct.unpack('H', data)[0]
blub.buttons.insert(0,int(button/2048))
button-=(int(button/2048))*button
blub.buttons.insert(0,int(button/1024))
button-=(int(button/1024))*button
blub.buttons.insert(0,int(button/512))
button-=(int(button/512))*button
blub.buttons.insert(0,int(button/256))
button-=(int(button/256))*button
blub.buttons.insert(0,int(button/128))
button-=(int(button/128))*button
blub.buttons.insert(0,int(button/64))
button-=(int(button/64))*button
blub.buttons.insert(0,int(button/32))
button-=(int(button/32))*button
blub.buttons.insert(0,int(button/16))
button-=(int(button/16))*button
blub.buttons.insert(0,int(button/8))
button-=(int(button/8))*button
blub.buttons.insert(0,int(button/4))
button-=(int(button/4))*button
blub.buttons.insert(0,int(button/2))
button-=(int(button/2))*button
blub.buttons.insert(0,int(button))
print blub
blub.axes=list()
blub.buttons=list()
self.ser.close()
def comapre_single_channel(data):
global k
last_row = csv_data[k-frequency_offset]
lat = Pose2D()
lat.x = float(last_row[1])
lat.y = convert_ppm(data.channels[3])
lat.theta = 0
latency_ch.publish(lat)
if (k < len(csv_data)):
row = csv_data[k]
msg = Joy()
msg.axes = (float(row[1]),float(row[2]),float(row[3]),float(row[4]),float(row[5]),float(row[6]),float(row[7]))
msg.buttons = (0,0,0,0,0,0,0,0,0,0,0)
k = k + 1
pub.publish(msg)
else:
k = 0
def main():
rospy.sleep(1)
rospy.init_node('trackpoint_joy')
trackpoint_joy_pub = rospy.Publisher('/trackpoint/joy', Joy)
tp_file = open( "/dev/input/mice", "rb" )
while not rospy.is_shutdown():
buf = tp_file.read(3)
button = ord( buf[0] )
bLeft = button & 0x1
bMiddle = ( button & 0x4 ) > 0
bRight = ( button & 0x2 ) > 0
x,y = struct.unpack( "bb", buf[1:] )
rospy.loginfo("L:%d, M: %d, R: %d, x: %d, y: %d\n" % (bLeft, bMiddle, bRight, x, y) )
joy_msg = Joy()
joy_msg.header.stamp = rospy.Time.now()
joy_msg.axes = [x, y]
joy_msg.buttons = [bLeft, bMiddle, bRight]
trackpoint_joy_pub.publish(joy_msg)
# rospy.sleep(0.1)
tp_file.close();
def main():
pub = rospy.Publisher('ernest_sensors/imu', Imu, queue_size=10)
eyes = rospy.Publisher('ernest_sensors/eyes', Range, queue_size=10)
nunchuck = rospy.Publisher('ernest_sensors/joy', Joy, queue_size=10)
nunchuck_giro = rospy.Publisher('ernest_sensors/joy_giro', Imu, queue_size=10)
comm = ArduinoCommunicator()
rospy.init_node('ernest_sensors')
for line in comm.loop():
print line
try:
x, y, z, dis, joy_x, joy_y, giro_x, giro_y, giro_z, but_c, but_z = line.split(",")
imu = Imu()
imu.header.frame_id = "imu"
imu.linear_acceleration.x = -float(x)
imu.linear_acceleration.y = float(z)
imu.linear_acceleration.z = float(y)
r = Range()
r.header.frame_id = "imu"
r.radiation_type = 1
r.field_of_view = 0.5
r.min_range = 0.20
r.max_range = 3
r.range = float(dis)/100.0
j = Joy()
j.axes = [
maprange(float(joy_x), 25, 226, -1, 1),
maprange(float(joy_y), 32, 223, -1, 1)
]
j.buttons = [int(but_c), int(but_z)]
imu2 = Imu()
imu2.header.frame_id = "imu"
imu2.linear_acceleration.x = (float(giro_y) - 512) / 512.0
imu2.linear_acceleration.y = -(float(giro_x) - 512) / 512.0
imu2.linear_acceleration.z = -(float(giro_z) - 512) / 512.0
pub.publish(imu)
eyes.publish(r)
nunchuck.publish(j)
nunchuck_giro.publish(imu2)
except ValueError:
continue
def diff(data):
global k
last_row = csv_data[k-frequency_offset]
diff = Quaternion()
diff.x = round(float(last_row[1])-convert_ppm(data.channels[3]),4)
diff.y = round(float(last_row[2])-convert_ppm(data.channels[1]),4)
diff.z = round(float(last_row[4])-convert_ppm(data.channels[0]),4)
diff.w = round(float(last_row[5])+convert_ppm(data.channels[2]),4)
testing_pub.publish(diff)
if (k < len(csv_data)):
row = csv_data[k]
msg = Joy()
msg.axes = (float(row[1]),float(row[2]),float(row[3]),float(row[4]),float(row[5]),float(row[6]),float(row[7]))
msg.buttons = (0,0,0,0,0,0,0,0,0,0,0)
k = k + 1
pub.publish(msg)
else:
k = 0
def publish(self, active_keys):
logdebug("Publishing!")
# in_event = self._device.read_one()
# while in_event is None:
# # rospy.sleep(0.1)
# in_event = self._device.read_one()
# if in_event is None:
# continue
# if in_event.type == ecodes.EV_KEY:
# break
# else:
# in_event = None
# if in_event is None:
# return
# if in_event.type == ecodes.EV_KEY:
msg = Joy(header=Header(stamp=Time.now()))
msg.axes = [0.0] * self._axes_count
msg.buttons = [0] * self._button_count
# active_keys = self._device.active_keys(verbose=False)
loginfo("active_keys : {}".format(active_keys))
for k in active_keys:
msg.buttons[key_map[k]] = 1
loginfo("msg.buttons : {}".format(msg.buttons))
self._pub.publish(msg)
def backwards(pub): msg = Joy() msg.header.stamp = rospy.Time.now() valueAxe = 0.0 valueButton = 0 walking_time = 30 #3 seconds for i in range (0, 20): msg.axes.append(valueAxe) for e in range (0, 17): msg.buttons.append(valueButton) rate = rospy.Rate(10) time.sleep(1) msg.axes[1] = -1 i=0 bo=True print "WALKING BACKWARDS" while not rospy.is_shutdown() and bo: i=i+1 if (i>walking_time): bo=False msg.axes[1] = 0 pub.publish(msg) rate.sleep()
def main(device_name, autorepeat_rate, frame_id):
rospy.loginfo("reading %s" % (device_name))
joy_pub = rospy.Publisher('joy', Joy)
with open(device_name, "rb" ) as tp_file:
prev_bLeft = 0
prev_bMiddle = 0
prev_bRight = 0
while not rospy.is_shutdown():
if autorepeat_rate == 0:
buf = tp_file.read(3)
else:
r,w,e = select.select([tp_file], [], [], 1.0 / autorepeat_rate)
if tp_file in r:
buf = os.read(tp_file.fileno(), 3)
else:
buf = None
if buf == None:
x, y = (0, 0)
bLeft = prev_bLeft
bMiddle = prev_bMiddle
bRight = prev_bRight
else:
button = ord( buf[0] )
bLeft = button & 0x1
bMiddle = ( button & 0x4 ) > 0
bRight = ( button & 0x2 ) > 0
x,y = struct.unpack( "bb", buf[1:] )
joy_msg = Joy()
joy_msg.header.stamp = rospy.Time.now()
joy_msg.header.frame_id = frame_id
joy_msg.axes = [x, y]
joy_msg.buttons = [bLeft, bMiddle, bRight]
joy_pub.publish(joy_msg)
prev_bLeft = bLeft
prev_bMiddle = bMiddle
prev_bRight = bRight
def right(pub): msg = Joy() msg.header.stamp = rospy.Time.now() valueAxe = 0.0 valueButton = 0 turning_time = 30 #3 seconds for i in range (0, 20): msg.axes.append(valueAxe) for e in range (0, 17): msg.buttons.append(valueButton) rate = rospy.Rate(10) time.sleep(1) msg.axes[2] = -1 i=0 bo=True print "TURNING RIGHT" while not rospy.is_shutdown() and bo: i=i+1 if (i>turning_time): bo=False msg.axes[2] = 0 pub.publish(msg) rate.sleep()
def backwards(pub):
msg = Joy()
msg.header.stamp = rospy.Time.now()
valueAxe = 0.0
valueButton = 0
walking_time = 30 #3 seconds
for i in range(0, 20):
msg.axes.append(valueAxe)
for e in range(0, 17):
msg.buttons.append(valueButton)
rate = rospy.Rate(10)
time.sleep(1)
msg.axes[1] = -1
i = 0
bo = True
print "WALKING BACKWARDS"
while not rospy.is_shutdown() and bo:
i = i + 1
if (i > walking_time):
bo = False
msg.axes[1] = 0
pub.publish(msg)
rate.sleep()
def main():
print "Started"
rospy.init_node("mock_server", anonymous=True)
joy_pub = rospy.Publisher("/joy", Joy)
joy_msg = Joy()
for i in range(7):
joy_msg.axes.append(0.0)
for j in range(12):
joy_msg.buttons.append(0)
joy_msg.axes[6] = 1
# rospy.loginfo('joystick vehicle controller starting')
print "Setting Host"
# myHost = '192.168.88.202'
# myPort = 1234
myHost = rospy.get_param("/ip", "192.168.88.202")
myPort = rospy.get_param("/port", 8080)
print "My Host : " + myHost
print "My Port : " + str(myPort)
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # create a TCP socket
try:
s.bind((myHost, myPort))
except socket.error, msg:
print "Bind failed. Error Code : " + str(msg[0]) + " Message " + msg[1] # bind it to the server port
def handler(data):
try:
recived = byteify(json.loads(data))
if 'trans' in recived and 'rot' in recived:
# Send joystic data
joy = Joy()
joy.axes = (recived['trans'] + recived['rot'])
joy_pub.publish(joy)
# Send twists data
twist = Twist()
twist.angular.x = recived['rot'][0]
twist.angular.y = recived['rot'][1]
twist.angular.z = recived['rot'][2]
twist.linear.x = recived['trans'][0]
twist.linear.y = recived['trans'][1]
twist.linear.z = recived['trans'][2]
twist_pub.publish(twist)
except AttributeError, e:
print e
def handler(data):
try:
recived = byteify(json.loads(data))
if 'trans' in recived and 'rot' in recived:
# Send joystic data
joy = Joy()
joy.axes = (recived['trans'] + recived['rot'])
joy_pub.publish(joy)
# Send twists data
twist = Twist()
twist.angular.x = recived['rot'][0]
twist.angular.y = recived['rot'][1]
twist.angular.z = recived['rot'][2]
twist.linear.x = recived['trans'][0]
twist.linear.y = recived['trans'][1]
twist.linear.z = recived['trans'][2]
twist_pub.publish(twist)
except AttributeError, e:
print e
def set_axes_velocity(self, req):
""" Set all joystick axes to velocity control"""
data = Joy()
rospy.loginfo("%s: Set all axis to velocity", self.name)
# Set all axis at 0.0 and set control to velocity for all axes
for i in range(12):
data.axes.append(0.0)
if i < 6:
data.buttons.append(0)
else:
data.buttons.append(1)
self.map_ack_data_callback(data)
return EmptyResponse()
def __init__(self):
self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.listener = TransformListener()
rospy.Subscriber("joy", Joy, self._joyChanged)
rospy.Subscriber("cmd_vel_telop", Twist, self._cmdVelTelopChanged)
self.cmd_vel_telop = Twist()
#self.pidX = PID(20, 12, 0.0, -30, 30, "x")
#self.pidY = PID(-20, -12, 0.0, -30, 30, "y")
self.pidX = PID(20, 12, 0.0, -30, 30, "x")
self.pidY = PID(20, 12, 0.0, -30, 30, "y")
self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z")
self.pidYaw = PID(-50.0, 0.0, 0.0, -200.0, 200.0, "yaw")
self.state = Controller.Manual
self.targetZ = 1
self.lastJoy = Joy()
def right(pub):
msg = Joy()
msg.header.stamp = rospy.Time.now()
valueAxe = 0.0
valueButton = 0
turning_time = 30 #3 seconds
for i in range(0, 20):
msg.axes.append(valueAxe)
for e in range(0, 17):
msg.buttons.append(valueButton)
rate = rospy.Rate(10)
time.sleep(1)
msg.axes[2] = -1
i = 0
bo = True
print "TURNING RIGHT"
while not rospy.is_shutdown() and bo:
i = i + 1
if (i > turning_time):
bo = False
msg.axes[2] = 0
pub.publish(msg)
rate.sleep()
def process(self):
if self.rc_data is None or self.w_q_b is None:
return
eulers = tfs.euler_from_quaternion(self.w_q_b, 'rzyx')
yaw = eulers[0]
des_roll = self.rc_data['roll'] * ROLL_PITCH_SCALE
des_pitch = -self.rc_data['pitch'] * ROLL_PITCH_SCALE
des_yawrate = self.rc_data['yaw'] * YAW_SCALE
if (abs(des_yawrate) < 1.0):
des_yawrate = 0.0
# normalized to 0 ~ 1
thr_from_rc = (self.rc_data['thrust'] + 1.0) / 2.0
# map to 10~100
thr_from_rc = 10.0 + 90.0 * thr_from_rc
if (self.rc_data['gear'] < GEAR_SHIFT_VALUE):
pass
else:
self.des_thrust = thr_from_rc
rospy.loginfo(
"rc[{:.2f}] rcmap[{: 3.0f}] ctrl[{: 3.0f}] yawrate[{:.2f}]".format(
self.rc_data['thrust'], thr_from_rc, self.des_thrust,
des_yawrate))
joy_msg = Joy()
joy_msg.header.stamp = rospy.Time.now()
joy_msg.header.frame_id = "FRD"
joy_msg.axes = [
des_roll, des_pitch, self.des_thrust, des_yawrate, VERT_THRUST,
YAW_MODE_RATE
]
self.ctrl_pub.publish(joy_msg)
def taker(data):
joy_pub = rospy.Publisher('/joy', Joy, queue_size=10)
joy_data = Joy()
img = ic.imgmsg_to_opencv(data)
config = Config()
image_process = ImageProcess()
IImage = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
IImage = cv2.resize(IImage, (config.image_size[0], config.image_size[1]))
IImage = image_process.process(IImage)
#cv2.imwrite('balu.jpg', IImage)
#drive_run = DriveRun(sys.agrv[1])
drive_run = DriveRun(
'/home/mir-lab/Desktop/Balu_Autodrive/2018-06-14-15-16-03')
prediction = drive_run.run(IImage)
#print(prediction)
#print(np.shape(prediction))
#print(type(prediction))
if (prediction[0][0] > 0.3):
#print("1")
prediction[0][0] = 1
elif (prediction[0][0] < -0.3):
#print("2")
prediction[0][0] = -1
else:
#print("3")
prediction[0][0] = 0
#new_pred = prediction.astype(np.float)
#new_predd = np.asscalar(np.array([prediction]))
#print(type(new_predd))
#print(np.shape(new_predd))
#new_predd = joy_data.axes.append(0)
#0.25 = joy_data.axes.append(3)
#print(new_predd)
#print(prediction[0][0])
joy_data.axes = [prediction[0][0], 0, 0, 0.05, 0, 0]
joy_pub.publish(joy_data)
def __init__(self):
self.settings_ = termios.tcgetattr(sys.stdin)
self.pub_B = rospy.Publisher('/AizuSpiderBB/joy', Joy, queue_size=1)
self.pub_A = rospy.Publisher('/AizuSpiderAA/joy', Joy, queue_size=1)
self.robo_num = 0
#self.pub_A = rospy.Publisher('robotA', Joy, queue_size = 1)
#self.pub_B = rospy.Publisher('robotB', Joy, queue_size = 1)
self.stop_ = rospy.get_param("~stop", 0.5)
self.button_flg = [0] * 11
self.joy_axis = [0] * 6
self.hand_status = 0
self.motion = 0
self.start_flg = 1
pygame.init()
pygame.joystick.init()
try:
self.j = pygame.joystick.Joystick(0) # create a joystick instance
self.j.init() # init
print('Joystick: ' + self.j.get_name())
print('bottun : ' + str(self.j.get_numbuttons()))
pygame.event.get()
except pygame.error:
print('No connect joystick')
print('exit')
pygame.quit()
sys.exit()
finally:
msg = Joy()
msg.axes = [0] * 8
msg.buttons = [0] * 11
self.pub_B.publish(msg)
self.pub_A.publish(msg)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings_)
def ready(pub): msg = Joy() msg.header.stamp = rospy.Time.now() valueAxe = 0.0 valueButton = 0 standup_time = 20 #2 seconds for i in range (0, 20): msg.axes.append(valueAxe) for e in range (0, 17): msg.buttons.append(valueButton) rate = rospy.Rate(10) time.sleep(1) msg.buttons[3] = 1 i=0 bo=True print "STAND_UP" while not rospy.is_shutdown() and bo: i=i+1 if (i>standup_time): bo=False msg.buttons[3] = 0 pub.publish(msg) rate.sleep()
def ready(pub):
msg = Joy()
msg.header.stamp = rospy.Time.now()
valueAxe = 0.0
valueButton = 0
standup_time = 20 #2 seconds
for i in range(0, 20):
msg.axes.append(valueAxe)
for e in range(0, 17):
msg.buttons.append(valueButton)
rate = rospy.Rate(10)
time.sleep(1)
msg.buttons[3] = 1
i = 0
bo = True
print "STAND_UP"
while not rospy.is_shutdown() and bo:
i = i + 1
if (i > standup_time):
bo = False
msg.buttons[3] = 0
pub.publish(msg)
rate.sleep()
def callback(self, msg):
buttons = {}
axes = {}
for i in range(len(msg.buttons)):
buttons[self.buttons_map[i]] = msg.buttons[i]
for j in range(len(msg.axes)):
axes[self.axes_map[j]] = msg.axes[j]
# TODO: Buttons to change control mode, should be a service server between this and guidance
surge = axes['vertical_axis_left_stick'] * self.surge_scaling
sway = axes['horizontal_axis_left_stick'] * self.sway_scaling
heave = (axes['RT'] - axes['LT']) / 2 * self.heave_scaling
roll = (buttons['RB'] - buttons['LB']) * self.roll_scaling
pitch = axes['vertical_axis_right_stick'] * self.pitch_scaling
yaw = axes['horizontal_axis_right_stick'] * self.yaw_scaling
joystick_msg = Joy()
joystick_msg.axes = [surge, sway, heave, roll, pitch, yaw]
self.pub.publish(joystick_msg)
def motion_interface():
rospy.init_node('motion_interface', anonymous = False)
interface.flushInput()
# Subscribe to velocity and direction messages
rospy.Subscriber("/motion_interface/velocity", Float32, velocity_callback)
rospy.Subscriber("/motion_interface/direction", Float32, direction_callback)
# Perform a loop checking on the enabled status of the drivers
while not rospy.is_shutdown():
# Build a message to send to the controller (request packet)
message = json.dumps({'request' : ['enabled', 'throttle', 'steering']});
interface.write(message)
interface.write('\r')
#rospy.loginfo(message)
# Fetch a JSON message from the controller and process (or atleast attempt to)
try:
_object = json.loads(interface.readline());
enabledPublisher.publish(Bool(_object['enabled']))
_joy = Joy()
_joy.header = rospy.Header()
_joy.axes.append(_object['throttle'])
_joy.axes.append(_object['steering'])
joystickPublisher.publish(_joy)
except json.decoder.JSONDecodeError:
enabledPublisher.publish(Bool(False))
rospy.logwarn("Invalid message received")
except: pass
rospy.sleep(0.1);
# Perform closing operations (like stopping motors)
message = json.dumps({'velocity' : 0.0, 'direction' : 0.0})
interface.write(message)
interface.write('\r')
def run_complete_experiment(self, length):
period = 2 * length
t0 = rospy.get_time()
while rospy.get_time() - t0 < length:
if rospy.is_shutdown():
break
w_cmd = self.get_w_cmd(t0, period)
phi_cmd = self.get_phi_cmd(t0, period)
theta_cmd = self.get_theta_cmd(t0, period)
psi_dot_cmd = self.get_psi_dot_cmd(t0, period)
# 0x02 + 0x08 = 0x0a
axes = [phi_cmd, theta_cmd, w_cmd, psi_dot_cmd, 0x0a]
msg = Joy(Header(), axes, [])
self.publisher.publish(msg)
self.rate.sleep()
def __init__(self, speed, rate):
self.speed = speed
rospy.init_node("Joystick_ramped")
rospy.Subscriber("joy", Joy, self.callback)
self.publisher = rospy.Publisher("a1_joy/joy_ramped",
Joy,
queue_size=10)
self.rate = rospy.Rate(rate)
# target
self.target_joy = Joy()
self.target_joy.axes = [0., 0., 1., 0., 0., 1., 0., 0.]
self.target_joy.buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
# last
self.last_joy = Joy()
self.last_joy.axes = [0., 0., 1., 0., 0., 1., 0., 0.]
self.last_joy.buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.last_send_time = rospy.Time.now()
while not rospy.is_shutdown():
self.publish_joy()
self.rate.sleep()
def keyCatcher(host):
pub = rospy.Publisher('/' + host + '/joy', Joy, queue_size=1)
rospy.init_node('joy-cli', anonymous=True)
while not rospy.is_shutdown():
direction = raw_input('Enter direction(a,w,s,d)--> ')
if direction == 'w':
axes = [0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
elif direction == 's':
axes = [0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
elif direction == 'd':
axes = [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0]
elif direction == 'a':
axes = [0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0]
else:
axes = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
msg = Joy(header=None, axes=axes, buttons=None)
pub.publish(msg)
rospy.sleep(0.5)
axes = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
msg = Joy(header=None, axes=axes, buttons=None)
pub.publish(msg)
def callback(self, message):
throttle_fwd = 0.1
throttle_bwd = 0.0
steer = message.data
out_msg = Joy()
out_msg.axes = [0.0] * 6
out_msg.axes[0] = steer
out_msg.axes[4] = throttle_fwd
out_msg.buttons = [0] * 12
self.pub.publish(out_msg)
def __init__(self):
rospy.init_node("vehicle", anonymous=False)
rospy.loginfo("======== Vehicle Node initialized ========")
self.joy = Joy()
self.is_upright = Bool()
self.sonic_dist = 0.0
self.gps = NavSatFix()
self.local_pose = Pose()
self.rate = rospy.Rate(20.0)
self.zoom_factor = 10000
self.pmotor_pub = rospy.Publisher("/power_motor",
Float32,
queue_size=10)
self.smotor_pub = rospy.Publisher("/steer_motor",
Float32,
queue_size=10)
self.local_pose_pub = rospy.Publisher("/local_pose",
Pose,
queue_size=10)
self.joy_sub = rospy.Subscriber("/joy", Joy, self.joy_cb)
self.tilt_ball_sub = rospy.Subscriber("/tilt_ball_switch", Bool,
self.tilt_ball_cb)
self.sonic_dist_sub = rospy.Subscriber("/ultrasonic_sensor", Float64,
self.sonic_dist_cb)
self.gps_sub = rospy.Subscriber("/gps", NavSatFix, self.gps_cb)
self.manual_srv = rospy.Service("manual_control", ManualControl,
self.manual_control)
self.autonav_srv = rospy.Service("autonav", AutoNav, self.autonav)
rospy.on_shutdown(self.shutdownhook)
self.wait(100)
self.origin = Pose()
self.origin.position.x = self.gps.longitude * self.zoom_factor
self.origin.position.y = self.gps.latitude * self.zoom_factor
rospy.loginfo("======== originX: " + str(self.origin.position.x) +
", originY: " + str(self.origin.position.y) +
" ========")
self.pose_thread = threading.Thread(target=self.calc_local_pose)
self.pose_thread.daemon = True
self.pose_thread.start()
rospy.spin()
def __init__(self):
# Publisher to ardrone cmd_vel topic, can be run in namespace
self.cmdVelPub = rospy.Publisher("cmd_vel_real", Twist, queue_size=1)
self.takeoffPub = rospy.Publisher("takeoff", Empty, queue_size=1)
self.landPub = rospy.Publisher("land", Empty, queue_size=1)
self.resetPub = rospy.Publisher("reset", Empty, queue_size=1)
# Initialize joy and cmd_vel variables
self.joyData = Joy()
self.bebopCmdVelReal = Twist() # Publishing to real cmd_vel
self.bebopCmdVel = Twist() # Subscribing to user cmd_vel
self.overrideControl = 0
# Subscriber to joystick topic
rospy.Subscriber("/joy", Joy, self.JoyCallback, queue_size=1)
rospy.Subscriber("cmd_vel", Twist, self.CmdVelCallback, queue_size=1)
def __init__(self,
translation_mag=0.01,
rotation_mag=0.01,
trans_relative=False):
self.translation_mag = translation_mag
self.rotation_mag = rotation_mag
self.current_msg = Joy()
self.current_msg.axes = [0, 0, 1, 0, 0, 1, 0, 0]
self.current_msg.buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.trans_relative = trans_relative
self.available_keys = [
Joystick.KEY_A, Joystick.KEY_Y, Joystick.KEY_B, Joystick.KEY_X,
Joystick.KEY_START, Joystick.KEY_BACK, Joystick.KEY_LOG
]
self.keyCallback = None
self.forced_target = None
def __init__(self):
# Initialize the node 'RasPi'
rospy.init_node('RasPi')
# Specify and initialize the COM port Arduino is on
try:
self.ser = serial.Serial('/dev/ttyUSB0',
115200,
timeout=1,
write_timeout=1)
except:
self.ser = serial.Serial('/dev/ttyUSB1',
115200,
timeout=1,
write_timeout=1)
# new_state is a list formatted to be an instruction for the Arduino to execute.
# The list is cast to a bytearray, after being altered by an event, and sent over
# the serial port to the Arduino event buffer to be executed in scheduled time.
# Its format is: [start char,M1PWM,M2PWM,M3PWM,M1Dir,M2Dir,M3Dir,action,time(3),end char]
self.new_state = [17, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 19]
self.curr_PWMs = [
100, 80
] # List to hold current linear and rotational PWM values [lin,rot]
self.hat = [0, 0]
self.old_axes = [0.0, 0.0]
self.old_buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.msgQueued = False
self.joy = Joy()
self.num_btns = 12
rospy.Subscriber('joy', Joy, self.joy_callback)
# self.timer = 0 #-----------------DEBUG-----------------
while not rospy.is_shutdown(
): # While the exit button(#10) hasn't been pressed, continue
if self.msgQueued:
self.ser.write(
bytearray(self.new_state)
) # Sends new robot instruction over serial to Arduino
#self.timer = time.clock() - self.timer #-----------------DEBUG-----------------
self.ser.flush()
self.msgQueued = False
# sys.stdout.write(str(self.timer) + ",") #-----------------DEBUG-----------------
# sys.stdout.flush() #-----------------DEBUG-----------------
sleep(0.01)
def callback(timer_info):
image = rospy.wait_for_message('/lead/camera_node/image/compressed',
CompressedImage)
arr = np.fromstring(image.data, np.uint8)
img = cv2.imdecode(arr, cv2.IMREAD_COLOR) #CV_LOAD_IMAGE_COLOR
command = Joy()
bodies = nbody.detectMultiScale(img, 1.2, 1)
highestw = 0
count = 0
action_threshhold = 0
highesth = 0
i = 0
while i < 8:
command.axes.append(0)
i += 1
for x, y, w, h in bodies:
cv2.rectangle(img, (x, y), (x + w, y + h), (0, 0, 255), 1)
if w > highestw:
trackx = x
tracky = y
trackh = h
highesth = h
highestw = w
count += 1
if count > 0 and highesth > h_threshold and move:
cv2.rectangle(img, (trackx, tracky),
(trackx + highestw, tracky + trackh), (0, 255, 0), 1)
if trackx + highestw / 2 < int((camwidth / 2) - camwidth / 10):
command.axes[
3] = steer_at #*((trackx+highestw/2)-camwidth/2)/(camwidth/2)
elif trackx + highestw / 2 > int((camwidth / 2) + camwidth / 10):
command.axes[
3] = -1 * steer_at #*((trackx+highestw/2)-camwidth/2)/(camwidth/2)
if highestw < move_threshhold:
command.axes[1] = speed
elif highestw > move_threshhold:
command.axes[1] = -1 * speed
else:
command.axes[1] = 0
command.axes[3] = 0
detect_msg = bridge.cv2_to_imgmsg(img, 'bgr8')
image_pub.publish(detect_msg)
#cv2.imshow("detected", img)
steering_pub.publish(command)
def __init__(self):
# pygame screen which will print currently pressed buttons
pygame.init()
self.sc = pygame.display
self.screen = self.sc.set_mode((500, 150), 0, 10)
self.screen.fill((0, 0, 0))
self.sc.set_caption("Pressed keys")
self.f1 = pygame.font.SysFont("comicsansms", 24)
self.text_x0 = 10
self.text_y = 20
self.text_dx = 20
# topic "/keyboard"
self.pub = rospy.Publisher('/keyboard', Joy, queue_size=10)
# node named "pressed_keys"
rospy.init_node('pressed_keys', anonymous=True)
# message
self.joyState = Joy()
def __init__(self):
rospy.init_node('robot_controller', anonymous=True)
self.local_deg_pub = rospy.Publisher('/uav/input/rateThrust',
RateThrust,
queue_size=10)
self.reset_pub = rospy.Publisher('/uav/collision', Empty, queue_size=1)
self.tf_pos_sub = rospy.Subscriber('/tf', TFMessage, self.tf_callback)
self.joy_sub = rospy.Subscriber('/control_nodes/joy', Joy,
self.joy_callback)
self.rate = rospy.Rate(30)
self.pose = TFMessage()
self.deg = RateThrust()
self.joy = Joy()
self.reset = Empty()
self.mode = 2 #default is mode 2
def cb_joy(self, joy_msg):
converted = self._converter.convert(joy_msg)
new_joy = Joy()
for key, val in converted.items():
key = key.lower()
if key.startswith('a'):
arr = new_joy.axes
elif key.startswith('b'):
arr = new_joy.buttons
else:
# Key not in the expected format. No problem, simply ignore it
continue
# Expected key format is aXX or bYY where XX and YY are integers
index = int(key[1:])
JoyConv._set_(arr, index, val)
self._pub.publish(new_joy)
def __init__(self, api=None, fprefix='streamer'):
print 'init'
self.api = api or API()
self.counter = 0
rospy.init_node("joy_simulate", anonymous=True)
self.pub = rospy.Publisher("/joy", Joy, queue_size=10)
self.msg = Joy()
self.msg.header.stamp = rospy.Time.now()
valueAxe = 0.0
valueButton = 0
for i in range(0, 20):
self.msg.axes.append(valueAxe)
for e in range(0, 17):
self.msg.buttons.append(valueButton)
self.rate = rospy.Rate(10)
time.sleep(1)
print 'init'
def __init__(self):
rospy.init_node('squirrel_joy', anonymous=False)
self.head_command = rospy.Publisher(
'/head_controller/joint_group_position_controller/command',
Float64,
queue_size=10)
self.neck_command = rospy.Publisher(
'/neck_controller/joint_group_position_controller/command',
Float64,
queue_size=10)
self.camera_command = rospy.Publisher(
'/camera_controller/joint_group_position_controller/command',
Float64,
queue_size=10)
self.face = rospy.ServiceProxy('face/emotion', DisplayScreen)
self.door = rospy.ServiceProxy('door_controller/command',
DoorController)
self.base_lights = rospy.Publisher('/light/command',
ColorRGBA,
queue_size=10)
self.head = Head(self.head_command)
self.neck = Neck(self.neck_command)
self.camera = Camera(self.camera_command)
self._button_bindings = [
self.leds_green, self.leds_red, self.leds_blue, self.expression,
self.head.move_left, self.head.move_right, self.open_door,
self.close_door, do_nothing, do_nothing, do_nothing
]
self._axes_bindings = [
do_nothing, do_nothing, do_nothing, do_nothing, do_nothing,
do_nothing, self.neck.move, self.camera.move
]
rospy.Subscriber('/joy', Joy, self.update_command)
self.previous_joy_message = Joy()
rospy.wait_for_service("/face/emotions")
expressions = rospy.ServiceProxy("/face/emotions", GetList)
self.expressions = expressions().list
rospy.wait_for_service("/sound/sounds")
sounds = rospy.ServiceProxy("/sound/sounds", GetList)
self.sounds = sounds().list
rospy.wait_for_service("/sound/play")
self.play_sound = rospy.ServiceProxy("/sound/play", PlaySound)
self.move_mouth = rospy.Publisher("/mouth/speak",
String,
queue_size=10)
self.expression(1)
def __init__(self):
self.msg = Joy()
self.torso_state=90
self.shoulder_state=45
self.elbow_state=15
self.wristrot_state=90
self.wristbend_state=10
self.claw_state=180
self.demo_button = 0
self.all_buttons = []
self.chat_pub = rospy.Publisher("chatter", String, queue_size=1000)
self.torso_pub = rospy.Publisher("torso_servo", UInt16, queue_size=10)
self.shoulder_pub = rospy.Publisher("shoulder_servo", UInt16, queue_size=10)
self.elbow_pub = rospy.Publisher("elbow_servo", UInt16, queue_size=10)
self.wristrot_pub = rospy.Publisher("wristrot_servo", UInt16, queue_size=10)
self.wristbend_pub = rospy.Publisher("wristbend_servo", UInt16, queue_size=10)
self.claw_pub = rospy.Publisher("claw_servo", UInt16, queue_size=10)
def node():
global joyData, max_rot, max_lin, n_robots, vel_msg
vel_msg = Twist()
joyData = Joy()
rospy.init_node('multirobot_joy', anonymous=False)
# fetching all parameters
n_robots = rospy.get_param('~n_robots', 3)
max_rot = rospy.get_param('~max_rot', 6.0)
max_lin = rospy.get_param('~max_lin', 1.5)
#-------------------------------------------
rospy.Subscriber("/joy", Joy, callBack)
rate = rospy.Rate(100)
while joyData.header.seq < 1:
pass
pubs = list()
for i in range(0, n_robots):
pubs.append(
rospy.Publisher('/robot_' + str(i + 1) +
'/mobile_base/commands/velocity',
Twist,
queue_size=10)
) #/keyop_vel_smoother/raw_cmd_vel /mobile_base/commands/velocity
zero = Twist()
vel_msg.linear.x = 0.0
vel_msg.linear.y = 0.0
vel_msg.linear.z = 0.0
vel_msg.angular.x = 0.0
vel_msg.angular.y = 0.0
vel_msg.angular.z = 0.0
#-------------------------------------------------------------------------
while not rospy.is_shutdown():
for i in range(0, n_robots):
if joyData.buttons[6 + i] > 0.0:
pubs[i].publish(vel_msg)
else:
pubs[i].publish(zero)
def __init__(self, rate=1):
self.control = {'l/r': 3, 'f/r': 1}
self.switch = {'start_record': 0, 'stop_record': 1}
rospy.init_node('joy_control')
rospy.Subscriber('joy', Joy, self.joy_cb)
self.cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size=0)
self.recording = False
self.joy_msg = Joy()
self.rate = rate
self.bagsRecorded = 0
self.base = rospy.get_param('~base_name', 'recording')
self.bag_recorder = None
print "Initialized"
def main():
global joy
pygame.midi.init()
rospy.init_node('midi_joy')
# parse the arg
argv = rospy.myargv()
if len(argv) == 0:
rospy.logfatal("You need to specify config yaml file")
sys.exit(1)
config_file = argv[1]
joy_pub = rospy.Publisher("/joy", Joy, queue_size=10)
autorepeat_rate = rospy.get_param("~autorepeat_rate", 0)
if autorepeat_rate == 0:
r = rospy.Rate(1000)
else:
r = rospy.Rate(autorepeat_rate)
with open(config_file, "r") as f:
config = yaml.load(f)
# open the device
controller = openMIDIInputByName(config["device_name"])
joy = Joy()
joy.axes = [0.0] * len(config["analogs"])
# automatically mapping to buttons from axes if it has NOTE_OFF or NOTE_ON MIDI commands
button_configs = [c for c in config["analogs"]
if c[0] == MIDICommand.NOTE_ON or c[0] == MIDICommand.NOTE_OFF]
if config.has_key("output"):
out_controller = openMIDIOutputByName(config["device_name"])
s = rospy.Subscriber("~set_feedback", JoyFeedbackArray, lambda msg: feedback_array_cb(out_controller, config, msg))
joy.buttons = [0] * len(button_configs)
while not rospy.is_shutdown():
joy.header.stamp = rospy.Time.now()
p = False
while controller.poll():
data = controller.read(1)
for elem_set in data:
p = True
(command, ind, val) = MIDIParse(elem_set)
try:
index = config["analogs"].index((command, ind))
joy.axes[index] = val
if command == MIDICommand.NOTE_ON or command == MIDICommand.NOTE_OFF:
button_index = button_configs.index((command, ind))
if val == 0.0:
joy.buttons[button_index] = 0
else:
joy.buttons[button_index] = 1
except:
rospy.logwarn("unknown MIDI message: (%d, %d, %f)" % (command, ind, val))
if (autorepeat_rate != 0) or p:
joy_pub.publish(joy)
r.sleep()
def __init__(self):
# self.planner_enabled = False
self.last_joy = Joy()
self.state = False
self.index = 0
self.time = time.time()
self.tests = []
for distance in DISTANCES:
for angle in ANGLES:
self.tests.append((distance, angle))
self.f = open(FILE_PATH, "wb")
self.writer = csv.writer(self.f, delimiter=',', quotechar='"')
self.writer.writerow(['DATA'])
self.writer.writerow([
'seq', 'stamp secs', 'stamp_nsecs', 'frame_id', 'radiation_type',
'field_of_view', 'min_range', 'max_range', 'range'
])
def __init__(self):
rospy.init_node("keyboard")
self.force_pub = rospy.Publisher('/NEXXUS_ROV/thrusters_input',
Float64MultiArray,
queue_size=10)
self.rate = rospy.Rate(20)
self.robot_pose = rospy.Subscriber("/NEXXUS_ROV/pose", Pose,
self.hold_thrust_callback)
self.joy_sub = rospy.Subscriber("/joy", Joy, self.joy_msg_callback)
self.joy_data = Joy()
self.vel_cmd = Float64MultiArray()
self.past_pose = Pose()
self.robot_pose = Pose()
self.joy_called = 0
self.set_pose = Pose()
def __init__(self):
rospy.init_node("uw_teleop_joint")
self.vel_pub = rospy.Publisher('/uwsim/joint_state_command',
JointState,
queue_size=30)
self.rate = rospy.Rate(30)
self.joy_sub = rospy.Subscriber("/joy", Joy, self.joy_msg_callback)
self.joy_data = Joy()
self.vel_cmd = JointState()
self.vel_cmd.name = [
'left1', 'left2', 'left3', 'left4', 'left5', 'left6', 'left_F_1',
'left_F_2', 'right1', 'right2', 'right3', 'right4', 'right5',
'right6', 'right_F_1', 'right_F_2'
]
self.vel_cmd.velocity = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
]
'z':[(1,-1),(0,1)],
'x':[(1,-1)],
'c':[(1,-1),(0,-1)],
'u': [(4,1),(3,1)],
'i': [(4,1)],
'o':[(4,1),(3,-1)],
'j':[(3,1)],
'k':[(4,0),(3,0)],
'l':[(3,-1)],
'm':[(4,-1),(3,1)],
',':[(4,-1)],
'.':[(4,-1),(3,-1)],
}
quit = False
mutex = threading.Lock()
joy = Joy()
def getKey():
tty.setraw(sys.stdin.fileno())
select.select([sys.stdin], [], [], 0)
key = sys.stdin.read(1)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
def publisher():
global joy, mutex, quit
pub = rospy.Publisher('/joy', Joy)
rate = rospy.Rate(10)
while not quit and not rospy.is_shutdown():
rate.sleep()
from crab_msgs.msg import BodyCommand
from crab_msgs.msg import BodyState
from crab_msgs.msg import GaitCommand
from crab_msgs.msg import LegIKRequest
from crab_msgs.msg import LegJointsState
from crab_msgs.msg import LegPositionState
from crab_msgs.msg import LegsJointsState
from sensor_msgs.msg import Joy
################
## INITIALIZE ##
################
pub = rospy.Publisher('/joy', Joy, queue_size=10)
msg = Joy()
msg.header.stamp = rospy.Time.now()
rate = rospy.Rate(10)
valueAxe = 0.0
valueButton = 0
for i in range (0, 20):
msg.axes.append(valueAxe)
for e in range (0, 17):
msg.buttons.append(valueButton)
#################
## TURN ##
#################
walking_time=float(seconds)+0.4 #manual time calibration
import roslib; roslib.load_manifest('aauship')
import rospy
from std_msgs.msg import String
from sensor_msgs.msg import Joy
import time
import os
## This is the joy tele operation node
class Joy(object):
def callback(self, data):
#print data.buttons
print time.time()
#pub.publish("control signals should be sent here")
pass
def run(self):
#pub = rospy.Publisher('lli_input', String)
sub = rospy.Subscriber('joy', Joy, self.callback)
rospy.init_node('joy_teleop')
rospy.spin() # Keeps the node running untill stopped
exit()
if __name__ == '__main__':
w = Joy()
w.run()
def run(self):
"""Loop that obtains the latest classic controller state, publishes the joystick data, and sleeps.
The Joy.msg message types calls for just two fields: float32[] axes, and int32[] buttons.
"""
self.threadName = "Classic Controller Joy topic Publisher"
try:
while not rospy.is_shutdown():
rospy.sleep(self.sleepDuration)
self.obtainWiimoteData()
if not self.wiistate.classicPresent:
continue
if self.pub is None:
self.pub = rospy.Publisher('/wiimote/classic', Joy)
rospy.loginfo("Wiimote Classic Controller joystick publisher starting (topic /wiimote/classic).")
(l_joyx, l_joyy) = self.wiistate.classicStickLeft
(r_joyx, r_joyy) = self.wiistate.classicStickRight
# scale the joystick to [-1, 1]
l_joyx = -(l_joyx-33)/27.
l_joyy = (l_joyy-33)/27.
r_joyx = -(r_joyx-15)/13.
r_joyy = (r_joyy-15)/13.
# create a deadzone in the middle
if abs(l_joyx) < .05:
l_joyx = 0
if abs(l_joyy) < .05:
l_joyy = 0
if abs(r_joyx) < .05:
r_joyx = 0
if abs(r_joyy) < .05:
r_joyy = 0
msg = Joy(header=None,
axes=[l_joyx, l_joyy,r_joyx, r_joyy],
buttons=None)
theButtons = [False,False,False,False,False,False,False,False,False,False,False,False,False,False,False]
theButtons[State.MSG_CLASSIC_BTN_X] = self.wiistate.classicButtons[CLASSIC_BTN_X]
theButtons[State.MSG_CLASSIC_BTN_Y] = self.wiistate.classicButtons[CLASSIC_BTN_Y]
theButtons[State.MSG_CLASSIC_BTN_A] = self.wiistate.classicButtons[CLASSIC_BTN_A]
theButtons[State.MSG_CLASSIC_BTN_B] = self.wiistate.classicButtons[CLASSIC_BTN_B]
theButtons[State.MSG_CLASSIC_BTN_PLUS] = self.wiistate.classicButtons[CLASSIC_BTN_PLUS]
theButtons[State.MSG_CLASSIC_BTN_MINUS] = self.wiistate.classicButtons[CLASSIC_BTN_MINUS]
theButtons[State.MSG_CLASSIC_BTN_LEFT] = self.wiistate.classicButtons[CLASSIC_BTN_LEFT]
theButtons[State.MSG_CLASSIC_BTN_RIGHT] = self.wiistate.classicButtons[CLASSIC_BTN_RIGHT]
theButtons[State.MSG_CLASSIC_BTN_UP] = self.wiistate.classicButtons[CLASSIC_BTN_UP]
theButtons[State.MSG_CLASSIC_BTN_DOWN] = self.wiistate.classicButtons[CLASSIC_BTN_DOWN]
theButtons[State.MSG_CLASSIC_BTN_HOME] = self.wiistate.classicButtons[CLASSIC_BTN_HOME]
theButtons[State.MSG_CLASSIC_BTN_L] = self.wiistate.classicButtons[CLASSIC_BTN_L]
theButtons[State.MSG_CLASSIC_BTN_R] = self.wiistate.classicButtons[CLASSIC_BTN_R]
theButtons[State.MSG_CLASSIC_BTN_ZL] = self.wiistate.classicButtons[CLASSIC_BTN_ZL]
theButtons[State.MSG_CLASSIC_BTN_ZR] = self.wiistate.classicButtons[CLASSIC_BTN_ZR]
msg.buttons = theButtons
measureTime = self.wiistate.time
timeSecs = int(measureTime)
timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
msg.header.stamp.secs = timeSecs
msg.header.stamp.nsecs = timeNSecs
try:
self.pub.publish(msg)
except rospy.ROSException:
rospy.loginfo("Topic /wiimote/classic closed. Shutting down Clas sender.")
exit(0)
#rospy.logdebug("Classic Controller state:")
#rospy.logdebug(" Classic Controller buttons: " + str(theButtons) + "\n Classic Controller axes: " + str(msg.axes) + "\n")
except rospy.ROSInterruptException:
rospy.loginfo("Shutdown request. Shutting down Clas sender.")
exit(0)
from crab_msgs.msg import BodyCommand
from crab_msgs.msg import BodyState
from crab_msgs.msg import GaitCommand
from crab_msgs.msg import LegIKRequest
from crab_msgs.msg import LegJointsState
from crab_msgs.msg import LegPositionState
from crab_msgs.msg import LegsJointsState
from sensor_msgs.msg import Joy
standup_time=20
################
## INITIALIZE ##
################
pub = rospy.Publisher('/joy', Joy, queue_size=10)
msg = Joy()
msg.header.stamp = rospy.Time.now()
rate = rospy.Rate(10)
valueAxe = 0.0
valueButton = 0
for i in range (0, 20):
msg.axes.append(valueAxe)
for e in range (0, 17):
msg.buttons.append(valueButton)
####################
## STAND UP ##
####################
msg.buttons[3] = 1
global Simulation
if self.status == DroneStatus.Flying or self.status == DroneStatus.GotoHover or self.status == DroneStatus.Hovering or Simulation:
self.pubCommand.publish(self.command)
#=======================#
# Some Classes #
#=======================#
from drone_video_display import DroneVideoDisplay
from PySide import QtCore, QtGui
#==========================#
# Button/Axis Defaults #
#==========================#
data = Joy()
data.buttons = [0]*12
data.axes = [0.0]*8
ShutDownNode = 0
ButtonEmergency = 1
TakeoffLand = 2
Up = 3
Down = 4
JoytoWay = 5
AxisRoll = 6
AxisPitch = 7
AxisYaw = 3
#============================#
# Scale Factor Defaults #
#============================#
def toPS3Msg(self):
joy = Joy()
joy.header = self.orig_msg.header
joy.buttons = [0] * 17
joy.axes = [0] * 20
if self.center:
joy.buttons[16] = 1
if self.select:
joy.buttons[0] = 1
if self.start:
joy.buttons[3] = 1
if self.L3:
joy.buttons[1] = 1
if self.R3:
joy.buttons[2] = 1
if self.square:
joy.axes[15] = -1.0
joy.buttons[15] = 1
if self.up:
joy.axes[4] = -1.0
joy.buttons[4] = 1
if self.down:
joy.axes[6] = -1.0
joy.buttons[6] = 1
if self.left:
joy.axes[7] = -1.0
joy.buttons[7] = 1
if self.right:
joy.axes[5] = -1.0
joy.buttons[5] = 1
if self.triangle:
joy.axes[12] = -1.0
joy.buttons[12] = 1
if self.cross:
joy.axes[14] = -1.0
joy.buttons[14] = 1
if self.circle:
joy.axes[13] = -1.0
joy.buttons[13] = 1
if self.L1:
joy.axes[10] = -1.0
joy.buttons[10] = 1
if self.R1:
joy.axes[11] = -1.0
joy.buttons[11] = 1
if self.L2:
joy.axes[8] = -1.0
joy.buttons[8] = 1
if self.R2:
joy.axes[9] = -1.0
joy.buttons[9] = 1
joy.axes[0] = self.left_analog_x
joy.axes[1] = self.left_analog_y
joy.axes[2] = self.right_analog_x
joy.axes[3] = self.right_analog_y
return joy
