def main(self):
try:
self.angle = 0
while not rospy.is_shutdown():
msg = Joy()
msg.axes = [0] * 8
msg.buttons = [0] * 11
# [3] if 1 arm above, -1 arm down
# [5] if 1 buck and -1 forward
# [4] if -1 left, 1 right
msg.axes[5] = -1.0
msg.axes[4] = self.angle * 2
if self.endflag:
msg.axes = [0] * 8
msg.buttons = [0] * 11
msg.axes[4] = -1
self.pub_.publish(msg)
self.rate.sleep()
except Exception as e:
print(e)
finally:
msg = Joy()
msg.axes = [0] * 8
msg.buttons = [0] * 11
self.pub_.publish(msg)
def main():
print "Howdy partner"
rospy.init_node('read_gps')
global dest_coords
sub2 = rospy.Subscriber('/dest_coords', Float32MultiArray,
callback_gps_dest)
if master_slave_mode:
pub0_ = rospy.Publisher('/gps_steer', Float32, queue_size=5)
pub1_ = rospy.Publisher('/gps_drive', Float32, queue_size=5)
pub2_ = rospy.Publisher('/gps_angle_to_dest', Float32, queue_size=5)
steer_msg = Float32()
drive_msg = Float32()
angle_to_dest_msg = Float32()
else:
pub0_ = rospy.Publisher('joy', Joy, queue_size=5)
msg = Joy()
while dest_coords is None:
if master_slave_mode:
drive_msg.data = 0
pub1_.publish(drive_msg)
else:
msg.axes = []
msg.axes.append(0)
msg.axes.append(0)
msg.axes.append(0)
pub0_.publish(msg)
sub0 = rospy.Subscriber('/unity_gps', Joy, callback_gps)
sub1 = rospy.Subscriber('/unity_heading', Joy, callback_heading)
rate = rospy.Rate(10)
while not rospy.is_shutdown() and not arrived:
rate.sleep()
if master_slave_mode:
steer_msg.data = steer
angle_to_dest_msg.data = angle_to_dest
drive_msg.data = drive
pub0_.publish(steer_msg)
pub1_.publish(drive_msg)
pub2_.publish(angle_to_dest_msg)
else:
#### Initialize joy msg every loop
msg.axes = []
msg.axes.append(steer)
msg.axes.append(drive)
msg.axes.append(drive)
pub0_.publish(msg)
print "destination arrived"
if master_slave_mode:
drive_msg.data = 0
pub1_.publish(drive_msg)
def goToTarget(self):
rate = rospy.Rate(1)
kill_code = 0
while not rospy.is_shutdown():
if(self.target !=""):
if (self.target != "" and self.target["prob"] > self.probabilityThreshlold):
#kill trajectory control
if(kill_code == 0):
os.system("rosnode kill go_home")
os.system("rosnode kill path_to_file")
kill_code = 1
#relative distance calculation
#right movement
movement_offset = Joy()
while(self.target != "" and self.target["x"] + (self.target["w"]/2) > self.aligntarget_x):
movement_offset.axes = [0.2, 0, 0]
self.setpoint.publish(movement_offset)
rospy.loginfo(movement_offset.axes)
rate.sleep()
#left movement
movement_offset = Joy()
while(self.target != "" and self.target["x"] + (self.target["w"]/2) < self.aligntarget_x):
movement_offset.axes = [-0.2, 0, 0]
self.setpoint.publish(movement_offset)
rospy.loginfo(movement_offset.axes)
rate.sleep()
#front movement
movement_offset = Joy()
while(self.target != "" and self.target["y"] + (self.target["h"]/2) < self.aligntarget_y):
movement_offset.axes = [0, 0.2, 0]
self.setpoint.publish(movement_offset)
rospy.loginfo(movement_offset.axes)
rate.sleep()
#back movement
movement_offset = Joy()
while(self.target != "" and self.target["y"] + (self.target["h"]/2) > self.aligntarget_y):
movement_offset.axes = [0, -0.2, 0]
self.setpoint.publish(movement_offset)
rospy.loginfo(movement_offset.axes)
rate.sleep()
if self.target != "" and (self.target["y"] + (self.target["h"]/2)> (self.aligntarget_y-0.1) and (self.target["y"] + (self.target["h"]/2)< self.aligntarget_y+0.1) and (self.target["x"] + (self.target["w"]/2)> self.aligntarget_x-0.1 ) and (self.target["x"] + (self.target["w"]/2)< self.aligntarget_x+0.1)):
rospy.loginfo("Fire!")
#GPIO : send PVM signal for relay to fire and close pwm
os.system("rosservice call /dji_sdk/mfio_config \"mode: 0 channel: 0 init_on_time_us: 2500 pwm_freq: 50\"")
rospy.loginfo("Reseting PWM!")
os.system("sleep 10; rosservice call /dji_sdk/mfio_config \"mode: 0 channel: 0 init_on_time_us: 500 pwm_freq: 50\"")
# calling gohome
os.system("sleep 50;./gohome.sh")
break
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 main():
print "Hello! Welcome to the Pinteraction demo"
print "This program allows you to actuate a pin of your choice to any height and for any duration"
pub = rospy.Publisher('/command', Joy, queue_size=20)
rospy.init_node('demo_interface')
msg = Joy()
while not rospy.is_shutdown():
input_row = raw_input("Please enter the row number (0 to 9): ")
input_col = raw_input("Please enter the column number (0 to 9): ")
input_height = raw_input("Please enter the height you would like to move to (in cm): ")
input_duration = raw_input("Please enter the time duration for the pin to stay there (in seconds): ")
try:
input_row = int(input_row)
input_col = int(input_col)
input_height = float(input_height)
input_duration = float(input_duration)
if input_row>9 or input_col>9 or input_row<0 or input_col<0 or input_height>10 or input_height<0 or input_duration<0:
print "Invalid data input. Please try again"
continue
except Exception, e:
print "Invalid data input. Please try again"
print str(e)
continue
msg.buttons = [input_row, input_col]
msg.axes = [input_height, input_duration]
pub.publish(msg)
print "The pin has been actuated."
input = raw_input("Enter x to exit the program, or any other key to actuate another pin. ")
if input == 'x' or input == 'X':
break
def rcCallback(self, joy_msg):
"""
This function is meant to receive an incoming joy message and analyse the info.
In this case every second incoming message is used to update the self.input_rc list of current 'human' remote data and then published to the flight controller.
:param joy_msg:
:return:
"""
self.counter_input += 1
if self.counter_input % 2 == 0:
self.counter_output += 1
joy_msg = Joy()
# Replay and override current rc
joy_msg.axes = self.input_rc[0:4]
joy_msg.buttons = self.input_rc[4:]
self.input_rc[0] = joy_msg.axes[0]
self.input_rc[1] = joy_msg.axes[1]
self.input_rc[2] = joy_msg.axes[2]
self.input_rc[3] = joy_msg.axes[3]
self.input_rc[4] = joy_msg.buttons[0]
self.input_rc[5] = joy_msg.buttons[1]
self.input_rc[6] = joy_msg.buttons[2]
self.input_rc[7] = joy_msg.buttons[3]
self.pub.publish(joy_msg)
if self.counter_input % 100 == 0:
print 'rc_tunnel_node.py >>', self.counter_input / (
time.time() -
self.start_time), 'rc inputs/sec ', self.counter_output / (
time.time() - self.start_time), 'rc outputs/sec'
def goToTarget(self):
p = 0.6
pAlt = 0.3
rate = rospy.Rate(5)
land_code = 0
while not rospy.is_shutdown():
if self.target != '' and land_code == 0:
# relative distance calculation
# XYZ movement
movement_offset = Joy()
while self.target != '':
xmask = round(self.target.x * p, 5)
ymask = round((self.target.y * -1) * p, 5)
zmask = round((self.aligntarget_z - self.target.z) * pAlt,
5)
movement_offset.axes = [xmask, ymask, zmask]
print(movement_offset.axes)
self.setpoint.publish(movement_offset)
if self.target != '' and round(
abs(ymask),
2) < 0.3 and round(abs(xmask), 2) < 0.3 and round(
abs(zmask), 2) < 0.3:
rospy.loginfo('Landing!')
command = rospy.ServiceProxy(
'/dji_sdk/drone_task_control', DroneTaskControl)
print(
command(DroneTaskControl._request_class.TASK_LAND))
land_code = 1
break # to stop following #place code here
rate.sleep()
def publisher():
# Looping at
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
pygame.event.pump()
# Empty values arrays
axes_values = []
buttons_values = []
# Initiate the two messages
msg_twist = Twist()
msg_joy = Joy()
# Iterate axes and populate value arrays
for axis in range(j.get_numaxes()):
axes_values.insert(axis, j.get_axis(axis))
for button in range(j.get_numbuttons()):
buttons_values.insert(button, j.get_button(button))
msg_twist.linear.x = axes_values[1] * (-1) * MAX_VELOCITY
msg_twist.angular.z = axes_values[2] * (-1) * MAX_ANGULAR_SPEED
msg_joy.axes = axes_values
msg_joy.buttons = buttons_values
cmd_vel_pub.publish(msg_twist)
joy_pub.publish(msg_joy)
# Preserve looping in specified rate
rate.sleep()
def move(self):
# Check if we have reached the next waypoint. If so, update
self.changeGoalPt()
self.v_max = rospy.get_param('maximum_velocity')
self.resetState()
velDes = self.getXdes()
# Pause to rotate after waypoint
if (rospy.Time.now() - self.last_waypoint_time).to_sec() < self.waypoint_wait_time:
velDes[:3] = [0,0,0]
# Publish Vector
joy_out = Joy()
joy_out.header.stamp = rospy.Time.now()
joy_out.axes = [velDes[0], velDes[1], velDes[2],velDes[3]]
self.vel_ctrl_pub_.publish(joy_out)
# Find future path
if (rospy.Time.now() - self.lastPathPub).to_sec() > 0.5:
self.lastPathPub = rospy.Time.now()
# publish path
self.findPath()
self.resetState()
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 default_ros_publish_callback(add, tag, stuff, source):
# ros << osc << gui
# input looks like incoming OSC: /gyrosc/gyro fff [0.48758959770202637, 0.06476165354251862, -0.19856473803520203] ('192.168.0.33', 57527)
if add in publishers.keys():
publisher = publishers[add]
if publisher[1] == 'Joy':
msg = Joy()
msg.axes = stuff[0:4]
msg.buttons = stuff[4:]
elif publisher[1] == 'Motor':
msg = Motor()
msg.motor0 = stuff[0]
msg.motor1 = stuff[1]
msg.motor2 = stuff[2]
msg.motor3 = stuff[3]
elif publisher[1] == 'GUI_cmd':
msg = GUI_cmd()
msg.gui_cmd_0 = stuff[0]
msg.gui_cmd_1 = stuff[1]
msg.gui_cmd_2 = stuff[2]
msg.gui_cmd_3 = stuff[3]
msg.gui_cmd_4 = stuff[4]
msg.gui_cmd_5 = stuff[5]
msg.gui_cmd_6 = stuff[6]
msg.gui_cmd_7 = stuff[7]
else:
print '>>> default_ros_publish_callback: not implemented msg_type:', publisher[
1]
return
publisher[0].publish(msg)
else:
print '>>> default_ros_publish_callback: not implemented topic:', add
def goToTarget(self):
p = 1
pAlt = 0.3
rate = rospy.Rate(15)
while not rospy.is_shutdown():
#rospy.loginfo(self.target)
if (type(self.target) is dict):
#print(self.target)
if (self.target["prob"] > self.probabilityThreshlold
and self.kill == 0):
#SDK Takes control
if (self.ctrlFlag == 0):
rospy.wait_for_service(
'/dji_sdk/sdk_control_authority')
control = rospy.ServiceProxy(
'/dji_sdk/sdk_control_authority',
SDKControlAuthority)
print(
control(SDKControlAuthority._request_class.
REQUEST_CONTROL))
self.ctrlFlag = 1
#Delta Calculation and Coordinate Transformation
x, y = self.cDeltaTransform(self.target)
# p-controller mask
xmask = round(x * p, 5)
ymask = round(y * p, 5)
zmask = 0 # localposition fusion required
movement_offset = Joy()
movement_offset.axes = [xmask, ymask, zmask]
rospy.loginfo(movement_offset.axes)
self.setpoint.publish(movement_offset)
rate.sleep()
def goToTarget(self):
p = 1
pAlt = 0.3
rate = rospy.Rate(15)
land_code = 0
while not rospy.is_shutdown():
if land_code == 0:
# relative distance calculation
# XYZ movement
movement_offset = Joy()
x, y = self.get_target_location()
while x != '' and y != '' and self.localDJI != '':
print(x, y)
xmask = round(x * p, 5)
ymask = round(y * p, 5)
#zmask = round((self.aligntarget_z-self.localDJI.z)*pAlt*-1,5)
zmask = round(self.localDJI.z * pAlt * -1, 5)
movement_offset.axes = [xmask, ymask, zmask]
print(movement_offset.axes)
self.setpoint.publish(movement_offset)
if abs(x) < 0.1 and abs(y) < 0.1 and round(abs(zmask),
2) < 0.1:
rospy.loginfo('Landed!')
command = rospy.ServiceProxy(
'/dji_sdk/drone_task_control', DroneTaskControl)
#print(command(DroneTaskControl._request_class.TASK_LAND))
#land_code = 1
#break # to stop following #place code here
x, y = self.get_target_location()
rate.sleep()
def getCoord(string):
string = string.data
s1, s2 = string.split(" ")
global x, y
x = float(s1)
y = float(s2)
rospy.loginfo("got dot")
if stopped:
road = sg.det_points_road(x, y, Rmap)
if road != 'Not on the road':
route = sg.find_route(cur_road, road.id)
s = ""
for i in range(len(route)):
s += str(route[i])
pub_route.publish(s)
msg = Joy()
msg.header.seq = 0
msg.header.stamp.secs = 0
msg.header.stamp.nsecs = 0
msg.header.frame_id = ''
msg.axes = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
msg.buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
msg.buttons[7] = 1
else:
rospy.loginfo("WARNING: requested dot is not on the road")
def _get_joy(self):
joy = Joy()
joy.header.frame_id = 'keyboard'
joy.header.stamp = rospy.Time.now()
joy.axes = self._axes
joy.buttons = self._buttons
return joy
def simulated_navigation(self,
forward=True,
duration=rospy.Duration(60, 0)):
"""
Walk for 1 meter
"""
response = self.ACTION_FAIL
start = rospy.Time.now()
self.set_current_location_as_home()
buttons = Joy()
buttons.header.stamp = rospy.Time.now()
buttons.header.frame_id = 'base_link'
direction = 1.0 if forward else -1.0
buttons.axes = [0.0, direction, 0.0, 0.0, 0.0, 0.0, 0.0]
buttons.buttons = [0, 0, 1, 0, 0, 0, 0, 0, 0]
for _ in range(3):
self._setpoint_pub.publish(buttons)
self._rate.sleep()
home_pos = np.array([self.home.geo.latitude, self.home.geo.longitude])
while (rospy.Time.now() - start < duration) and (
not rospy.is_shutdown()) and (not self.external_intervened):
cur_pos = np.array(
[self.global_pose.latitude, self.global_pose.longitude])
if np.linalg.norm(cur_pos - home_pos) > self._radius:
# self.embrace_pose()
response = self.ACTION_SUCCESS
break
if (rospy.Time.now() - start) > duration:
response = self.OUT_OF_DURATION
if self.external_intervened:
response = self.EXTERNAL_INTERVENTION
return response
def publish_joy(self):
t_now = rospy.Time.now()
# determine changes in state
buttons_change = array_equal(self.last_joy.buttons,
self.target_joy.buttons)
axes_change = array_equal(self.last_joy.axes, self.target_joy.axes)
# new message
if not (buttons_change and axes_change):
joy = Joy()
if not axes_change:
# do ramped_vel for every single axis
for i in range(len(self.target_joy.axes)):
joy.axes.append(
self.ramped_vel(self.last_joy.axes[i],
self.target_joy.axes[i],
self.last_send_time, t_now))
else:
joy.axes = self.last_joy.axes
joy.buttons = self.target_joy.buttons
self.last_joy = joy
self.publisher.publish(self.last_joy)
self.last_send_time = t_now
def publish(self, state):
curr_time = rospy.get_rostime()
# limit to 20hz
# print str((curr_time - self.last_pub_time).to_sec())
if (curr_time - self.last_pub_time).to_sec() < 0.05:
return
self.last_pub_time = curr_time
joy_msg = Joy()
joy_msg.header.stamp = rospy.Time.now()
joy_msg.buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(0, 17):
joy_msg.buttons[i] = state[i]
joy_msg.axes = [
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
0., 0.213169, 0.
]
for i in range(0, 4):
if state[i] <= 127:
joy_msg.axes[i] = (127 - state[i + 17]) / 127.0
else:
joy_msg.axes[i] = (state[i] - 127) / 128.0
for i in range(0, 12):
joy_msg.axes[i + 4] = -state[i + 21] / 255.0
self.joy_pub.publish(joy_msg)
def idle_controller():
joy_msg = Joy()
joy_msg.axes = [0.0 for i in range(8)]
joy_msg.buttons = [0 for i in range(15)]
joy_msg.axes[2] = 1.0
joy_msg.axes[5] = 1.0
return joy_msg
def hoverInPlace(self):
# Safety hovering
# Publish Vector, stay in place
joy_out = Joy()
joy_out.header.stamp = rospy.Time.now()
joy_out.axes = [0.0, 0.0, 0.0, 0.0]
self.vel_ctrl_pub_.publish(joy_out)
def publish_joy(self):
t_now = rospy.Time.now()
# determine changes in state
buttons_change = array_equal(self.last_joy.buttons, self.target_joy.buttons)
axes_change = array_equal(self.last_joy.axes, self.target_joy.axes)
# if the desired value is the same as the last value, there's no
# need to publish the same message again
if not(buttons_change and axes_change):
# new message
joy = Joy()
if not axes_change:
# do ramped_vel for every single axis
for i in range(len(self.target_joy.axes)):
if self.target_joy.axes[i] == self.last_joy.axes[i]:
joy.axes.append(self.last_joy.axes[i])
else:
joy.axes.append(self.ramped_vel(self.last_joy.axes[i],
self.target_joy.axes[i],self.last_send_time,t_now))
else:
joy.axes = self.last_joy.axes
joy.buttons = self.target_joy.buttons
self.last_joy = joy
self.publisher.publish(self.last_joy)
self.last_send_time = t_now
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 __init__(self):
self.settings_ = termios.tcgetattr(sys.stdin)
self.joy_pub = rospy.Publisher('/robot/joy', 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.joy_pub.publish(msg)
def callback(self, in_msg):
out_msg = Joy(header=in_msg.header)
map_axes = self.mappings["axes"]
map_btns = self.mappings["buttons"]
out_msg.axes = [0.0] * len(map_axes)
out_msg.buttons = [0] * len(map_btns)
in_dic = {"axes": in_msg.axes, "buttons": in_msg.buttons}
for i, exp in enumerate(map_axes):
try:
out_msg.axes[i] = self.evaluator.reval(exp, in_dic)
except NameError as e:
rospy.logerr(
"You are using vars other than 'buttons' or 'axes': %s" %
e)
except UnboundLocalError as e:
rospy.logerr("Wrong form: %s" % e)
except Exception as e:
raise e
for i, exp in enumerate(map_btns):
try:
if self.evaluator.reval(exp, in_dic) > 0:
out_msg.buttons[i] = 1
except NameError as e:
rospy.logerr(
"You are using vars other than 'buttons' or 'axes': %s" %
e)
except UnboundLocalError as e:
rospy.logerr("Wrong form: %s" % e)
except Exception as e:
raise e
self.pub.publish(out_msg)
def cbJoy(self, joy_msg):
out_msg = Joy()
out_msg.header = joy_msg.header
#
out_msg.axes = [0.] * 6
# A, B, X, Y, LB, RB, back, start, power, btn_stick_left, btn stick right
# 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10
out_msg.buttons = [0] * 10
# X -> A
out_msg.buttons[0] = joy_msg.buttons[0]
# circle -> B
out_msg.buttons[1] = joy_msg.buttons[2]
# square -> X
out_msg.buttons[2] = joy_msg.buttons[1]
# triangle -> Y
out_msg.buttons[3] = joy_msg.buttons[3]
# share -> start
out_msg.buttons[7] = joy_msg.buttons[8]
# logo -> Power
out_msg.buttons[8] = joy_msg.buttons[24]
# accelerator
out_msg.axes[1] = (joy_msg.axes[2] - joy_msg.axes[3]) / 2.
# steering wheel -> Left/Right Stick
out_msg.axes[3] = joy_msg.axes[0]
# up_down
out_msg.axes[2] = 1.
out_msg.axes[5] = 1.
if joy_msg.buttons[4] == 1 and joy_msg.buttons[5] == 0: # RT
out_msg.axes[5] = -joy_msg.axes[1]
elif joy_msg.buttons[4] == 0 and joy_msg.buttons[5] == 1: # LT
out_msg.axes[2] = -joy_msg.axes[1]
self.pub_joy.publish(out_msg)
def simulated_rotation(self, degree, duration=rospy.Duration(60, 0)):
"""
Rotate for [degree] degree
"""
response = self.ACTION_FAIL
start = rospy.Time.now()
heading = self.heading
offset = degree / 180. * np.pi
buttons = Joy()
buttons.header.stamp = rospy.Time.now()
buttons.header.frame_id = 'base_link'
buttons.axes = [0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0]
buttons.buttons = [0, 0, 1, 0, 0, 0, 0, 0, 0]
for _ in range(3):
self._setpoint_pub.publish(buttons)
self._rate.sleep()
while (rospy.Time.now() - start < duration) and (
not rospy.is_shutdown()) and (not self.external_intervened):
current_heading = self.heading
if abs(current_heading - heading) > offset:
self.sit_pose()
response = self.ACTION_SUCCESS
break
if (rospy.Time.now() - start) > duration:
response = self.OUT_OF_DURATION
if self.external_intervened:
response = self.EXTERNAL_INTERVENTION
return response
def set_x(self, change):
yaw = self.StreamAttitude.yaw_y
msg = Joy()
msg.axes = [change, 0, self.height, 0]
self.position_control.publish(msg)
time.sleep(1)
return
def set_z(self, change):
yaw = self.StreamAttitude.yaw_y
msg = Joy()
msg.axes = [0, change, self.height, 0] #double check these values
self.position_control.publish(msg)
time.sleep(1)
return
def comando_robo():
# Cria o publisher
pub = rospy.Publisher('roboComandoVelocidade', Joy, queue_size=10)
# Inicia o no
rospy.init_node('comando_robo', anonymous=True)
# Frequencia de envio dos comandos
rate = rospy.Rate(10)
# Loop inifinito para envio do comando
while not rospy.is_shutdown():
# Valores a serem enviados
vel_left = 1
vel_right = 2
# montando o array com as velocidades para os quatro motores
velocidades = [vel_left, vel_left, vel_right, vel_right]
# Monta a mensagem a ser enviada
mensagem = Joy()
mensagem.axes = velocidades
# Publica no no
pub.publish(mensagem)
# Envia uma mensagem ao usuario
rospy.loginfo('Vel L: ' + str(vel_left) + ' Vel R: ' + str(vel_right))
# Pausa o codigo pelo tempo especificado
rate.sleep()
def rush(self):
# Safety violation test, rush in x direction
# Publish Vector
joy_out = Joy()
joy_out.header.stamp = rospy.Time.now()
joy_out.axes = [self.v_max * 1.5, 0., 0., 0.]
self.vel_ctrl_pub_.publish(joy_out)
def main():
global drive
global steer
rospy.init_node('obs_avoid')
#Subscribers
sub0 = rospy.Subscriber('/lidar_drive', Float32, callback_lidar_drive)
sub1 = rospy.Subscriber('/gps_steer', Float32, callback_gps_steer)
sub2 = rospy.Subscriber('/gps_drive', Float32, callback_gps_drive)
sub3 = rospy.Subscriber('/lane_steer', Float32, callback_lane_steer)
sub4 = rospy.Subscriber('/gps_angle_to_dest', Float32,
callback_angle_to_dest)
sub4 = rospy.Subscriber('/vision_drive', Float32, callback_vision_drive)
#Publishers
pub_ = rospy.Publisher('joy', Joy, queue_size=5)
msg = Joy()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
#Basic choice for drive component, we pick
drive = determine_drive()
steer = determine_steer()
rate.sleep()
msg.axes = []
msg.axes.append(steer)
msg.axes.append(drive)
msg.axes.append(drive)
#rospy.loginfo(msg)
pub_.publish(msg)
def run(self):
joy = Joy()
joy.axes = (0, 0, 0, 0, 0, 0)
status = 0
try:
print (textwrap.dedent(TeleopKey.msg))
print (self.params_msg())
while True:
key = self.get_key()
if key in self.move_bindings.keys():
joy.axes = self.move_bindings[key]
elif key in self.claw_bindings.keys():
self.claw_bindings[key]()
elif key in self.param_bindings.keys():
self.params[self.param_bindings[key][0]]\
*= self.param_bindings[key][1]
if self.param_bindings[key][0] == 'drive_speed':
self.param_client.update_configuration(
{'DRIVE_SPEED': self.params['drive_speed']}
)
elif self.param_bindings[key][0] == 'turn_speed':
self.param_client.update_configuration(
{'TURN_SPEED': self.params['turn_speed']}
)
# Update params once now to make sure no params were
# set to invalid values.
server_config = self.param_client.get_configuration()
self.update_params(server_config)
if (status == 14):
print (textwrap.dedent(TeleopKey.msg))
print (self.params_msg())
status = (status + 1) % 15
else:
joy.axes = (0, 0, 0, 0, 0, 0)
if (key == '\x03'):
break
self.pub.publish(joy)
except Exception as e:
print('Something went wrong:')
for exception in traceback.format_exception_only(type(e), e):
print(exception)
traceback.print_exc()
finally:
joy.axes = (0, 0, 0, 0, 0, 0)
self.pub.publish(joy)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
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 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
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 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):
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 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 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():
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 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 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 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 talker1(x, y):
msg = Joy()
msg.axes = [0, 0, 0, -x, y, 0]
msg.buttons = [0,0,0]
pub2.publish(msg)
def twist_cb(msg): # twist -> joy callback res_ = hash_client_([msg.linear.x, msg.angular.z]) jmsg_ = Joy(); jmsg_.axes = res_.bin jmsg_.header.stamp = rospy.get_rostime(); pub_.publish(jmsg_)
while not quit and not rospy.is_shutdown():
rate.sleep()
with mutex:
joy.header.stamp = rospy.Time.now()
pub.publish(joy)
if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('joystick_keyboard_emulator')
print msg
try:
joy.axes = [0.0] * 10
joy.buttons = [0] * 10
thread = threading.Thread(target = publisher)
thread.start()
while not rospy.is_shutdown():
key = getKey()
with mutex:
if key in moveBindings.keys():
for (axis,action) in moveBindings[key]:
if action == 0:
joy.axes[axis] = 0.0
else:
joy.axes[axis] += action * scale
joy.axes[axis] = min(max(-1,joy.axes[axis]),1)
elif key in [str(x) for x in range(0,10)]:
joy.buttons[int(key)] = 1 - joy.buttons[int(key)]
from beachbot_pathgen import Generator
rospy.init_node("server_node", disable_signals=True)
pub2 = rospy.Publisher('joy', Joy)
pub3 = rospy.Publisher('states', State)
app = Flask(__name__)
@app.route("/hallo")
def hallo():
return "Hallo!"
msg1 = Joy()
msg1.buttons = [0,0,0,0,0,0,0]
msg1.axes = [0,0,0,0,0,0]
class WebsocketApp(WebSocketApplication):
broadcast_rate = rospy.Rate(18)
def on_open(self):
print("Websocket Open")
ros_t = threading.Thread(target=self.ros_thread)
ros_t.daemon = True
ros_t.start()
return
# self.broadcast_ros(["test"])
def on_message(self, message):
if type(message) is str:
msg = json.loads(message)
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 #
#============================#
ScaleRoll = 1.0
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Range
rospy.init_node("teleop_withRange")
pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
gJoy = Joy()
gJoy.axes = [0,0,0,0,0,0]
gRange = Range()
twist = Twist()
something_in_front = True
def callback_joy(joy):
global gJoy
gJoy = joy
def callback_range(ranges):
global gRange
gRange = ranges
rospy.Subscriber('range', Range, callback_range)
rospy.Subscriber('joy', Joy, callback_joy)
r = rospy.Rate(20)
#rospy.spin()
while not rospy.is_shutdown():
print "Object %f far away" % gRange.range
def talker2(index, is_pressed):
msg = Joy()
msg.axes = [1.2, 3.4]
msg.buttons = [0,0,0]
pub2.publish(msg)
