def mobility_action(message): #object to bytes
try:
data = unpack('2i18b', message)
print(time())
print(data[0])
if int(time()) != data[0]:
print("STALE COMMAND")
return
global multijoy_pub
msg = MultiJoy()
msg.source = 2
msg.njoys.data = 1
t_joy = Joy()
t_joy.header.stamp.secs = data[0]
t_joy.header.stamp.nsecs = data[1]
for i in range(2, 8):
t_joy.axes.append(float(float(data[i]) / float(127)))
for i in range(8, 20):
t_joy.buttons.append(data[i])
msg.joys.append(t_joy)
multijoy_pub.publish(msg)
print(msg)
print("PUBLISHED")
except:
print("Error in D")
def keyCatcher():
rospy.init_node('joy-cli')
pub = rospy.Publisher('~joy', Joy, queue_size=1)
while not rospy.is_shutdown():
axes = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
direction = input('Enter direction [a,w,s,d] to move; '
'[l] to start lane following; '
'[q] to stop lane following; '
'[e] to toggle emergency stop; '
'then press enter to execute --> ')
if direction == 'w':
axes[1] = 1.0
elif direction == 's':
axes[1] = -1.0
elif direction == 'd':
axes[3] = -1.0
elif direction == 'a':
axes[3] = 1.0
elif direction == 'l':
buttons[7] = 1
elif direction == 'q':
buttons[6] = 1
elif direction == 'e':
buttons[3] = 1
# publish joy message
msg = Joy(header=None, axes=axes, buttons=buttons)
pub.publish(msg)
rospy.sleep(0.5)
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 image_callback(self, image):
try:
cv_image = self.bridge.imgmsg_to_cv2(image, "bgr8")
gray_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
except CvBridgeError as e:
print(e)
turn_angle, collision_probability = self.dn(gray_image[::4, ::4])
self.current_velocity = self.alpha * self.current_velocity + (
1 - self.alpha) * (1.0 - collision_probability) * self.max_velocity
self.current_turn_angle = self.beta * self.current_turn_angle + (
1 - self.beta) * pi / 2.0 * turn_angle
angular_velocity = self.current_velocity * self.current_turn_angle / (
self.steering_ratio * self.wheel_base *
(1 +
self.slip_factor * self.current_velocity * self.current_velocity))
t = Twist()
t.linear.x = self.current_velocity
t.angular.z = angular_velocity
self.twist_pub.publish(t)
j = Joy()
j.header.stamp = rospy.get_rostime()
j.axes.append(self.current_velocity)
j.axes.append(0.0)
j.axes.append(0.0)
j.axes.append(angular_velocity)
self.drone_cmd_pub.publish(j)
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 image_callback(self, pic):
if self.netEnable:
joy = Joy()
cv2image = self.bridge.imgmsg_to_cv2(pic)
cv2image = cv2.resize(cv2image, (200, 150),
interpolation=cv2.INTER_CUBIC)
cv2image = cv2image[35:, :, :]
normed_img = cv2image.astype(dtype=np.float32) / 255.0
# normed_img = np.reshape(normed_img, (115, 200, 1))
steer = self.model.y_out.eval(session=self.sess,
feed_dict={
self.model.x: [normed_img],
self.model.keep_prob_fc1: 1.0,
self.model.keep_prob_fc2: 1.0,
self.model.keep_prob_fc3: 1.0,
self.model.keep_prob_fc4: 1.0
})
if abs(steer[0][0]) < 10e-2:
steer[0][0] = 0
joy.axes.append(steer[0][0])
joy.axes.append(1.0)
#joy.axes[1] = 1.0
#joy.axes[0] = steer[0][0]
print("steering angle = %s\n" % str(joy.axes[0]))
self.joy_pub.publish(joy)
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 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 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 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 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 goToBodyTarget(self,
roll=0,
pitch=0,
dz=0,
yaw=0,
duration=1,
post_sleep=0):
msg = Joy()
msg.axes.append(roll)
msg.axes.append(pitch)
msg.axes.append(dz)
msg.axes.append(yaw)
counter = 0
rate = rospy.Rate(5)
while not rospy.is_shutdown():
self.body_control.publish(msg)
rate.sleep()
counter += 1
if counter >= (duration * 5):
break
sleep(post_sleep)
return
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 __init__(self):
rospy.init_node('keyboard_to_joystick')
self.pub = rospy.Publisher("/joy", Joy, queue_size=50)
self.joy_msg = Joy()
self.main()
def image_callback(self, pic):
if self.netEnable:
joy = Joy()
cv2image = self.bridge.imgmsg_to_cv2(pic)
cv2image = cv2.resize(cv2image, (320, 240), interpolation=cv2.INTER_CUBIC)
cv2image = cv2image[50:170, :, :]
normed_img = cv2image.astype(dtype=np.float32) / 255.0
steer = self.model.y_out.eval(session=self.sess, feed_dict={self.model.x: [normed_img],
self.model.keep_prob_fc1: 1.0,
self.model.keep_prob_fc2: 1.0,
self.model.keep_prob_fc3: 1.0,
self.model.keep_prob_fc4: 1.0})
#Get feature maps and publish them for rviz
#x = self.model.h_conv5.eval(session=self.sess, feed_dict={self.model.x: [normed_img],
# self.model.keep_prob_fc1: 1.0,
# self.model.keep_prob_fc2: 1.0,
# self.model.keep_prob_fc3: 1.0,
# self.model.keep_prob_fc4: 1.0})
#x = x[0]
#y = x[:,:,0]
#for i in range(1,64):
# y += x[:,:,i]
#try:
# img = self.bridge.cv2_to_imgmsg(y, "32FC1")
# self.img_pub.publish(img)
#except CvBridgeError as e:
# print(e)
if abs(steer[0][0]) < 0.0001:
steer[0][0] = 0
joy.axes.append(steer[0][0])
joy.axes.append(1.0)
print("steering angle = %s\n" % str(joy.axes[0]))
self.joy_pub.publish(joy)
def __init__(self):
rospy.init_node('joy_to_ptu', anonymous=True)
rospy.Subscriber("joy", Joy, self.joyCallback)
rospy.Subscriber("cmd_vel", Twist, self.navCallback)
self.pubPTU = rospy.Publisher("/cmd_ptu", ptuMsg, queue_size=1)
self.joy_data = Joy()
self.ptuCmd = ptuMsg()
def camera_follow_trajectory(latent_reset_func,
waypoints,
metric,
waiting_for_next,
origin,
loop=False):
LRESET_BUTTON = 8
_joy_msg = Joy()
def update_func(traj_obj, msg):
nonlocal _joy_msg
if type(msg) == Joy:
if msg.buttons[
LRESET_BUTTON] and not _joy_msg.buttons[LRESET_BUTTON]:
# RESET
latent_reset_func()
_joy_msg = msg
else:
pass
traj = TrajectoryRos(
update_func,
[('/joy', Joy)],
'absolute',
waypoints,
metric, # moves on when func(obs, goal) < thresh
waiting_for_next,
origin,
loop=loop)
return traj
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 translate(self, x_target, y_target):
msg=Joy()
# vel=200 #must be small to avoid jerking, and secondly to avoid switching surface
# distance_threshold=0.1
x_error=(x_target-self.x0)*math.cos(self.yaw0)+(y_target-self.y0)*math.sin(self.yaw0)
y_error=-(x_target-self.x0)*math.sin(self.yaw0)+(y_target-self.y0)*math.cos(self.yaw0)
x_derivative = (x_error - self.pre_x_error) / self.del_T
y_derivative = (y_error - self.pre_y_error) / self.del_T
x_linear_vel = (self.p_lin * x_error) + (self.d_lin * x_derivative)
if x_linear_vel > self.lin_vel_thres:
x_linear_vel = self.lin_vel_thres
elif x_linear_vel < -self.lin_vel_thres:
x_linear_vel = -self.lin_vel_thres
x = self.bias + x_linear_vel
y_linear_vel = (self.p_lin * y_error) + (self.d_lin * y_derivative)
if y_linear_vel > self.lin_vel_thres:
y_linear_vel = self.lin_vel_thres
elif y_linear_vel < -self.lin_vel_thres:
y_linear_vel = -self.lin_vel_thres
y = self.bias + y_linear_vel
msg.buttons = [x, y, self.bias]
self.cmd_vel_pub.publish(msg)
self.pre_x_error = x_error
self.pre_y_error = y_error
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 __init__(self):
self.local_pose = Point(0.0, 0.0, 0.0)
self.joy_msg = Joy()
self.joy_msg.axes = [0.0, 0.0, 0.0]
# step size multiplies joystick input
self.joy_FACTOR = 2.0
# Fence params
self.fence_x_min = rospy.get_param('fence_min_x', 0.0)
self.fence_x_max = rospy.get_param('fence_max_x', 5.0)
self.fence_y_min = rospy.get_param('fence_min_y', 0.0)
self.fence_y_max = rospy.get_param('fence_max_y', 5.0)
# flags
self.home_flag = False
self.takeoff_flag = False
self.land_flag = False
self.disarm_flag = False
self.arm_flag = False
# Instantiate a setpoint topic structure
self.setp = PositionTarget()
# use position setpoints
self.setp.type_mask = int('01011111000', 2)
# get altitude setpoint from parameters
self.altSp = rospy.get_param("altitude_setpoint", 1.0)
self.setp.position.z = self.altSp
def rc_callback(data):
# PX4 SITL VTOL channels:
# 1 FR, ccw (900 - 2000)
# 2 RL, ccw
# 3 FL, cw
# 4 RR, cw (1000 - 2000)
# 5 pusher (1000 - 2000)
# 6 aileron (1000 - 2000)
# 7 aileron
# 8 elevator
FR_pwm = data.channels[0]
RL_pwm = data.channels[1]
FL_pwm = data.channels[2]
RR_pwm = data.channels[3]
pusher_pwm = data.channels[4]
aileron1_pwm = data.channels[5]
aileron2_pwm = data.channels[6]
elevator_pwm = data.channels[7]
joy = Joy()
# Sim:
# FR, cw, rate, rpm (Front right motor speed)
# RL, cw, rate, rpm (Rear left motor speed)
# FL, ccw, rate, rpm (Front left motor speed)
# RR, ccw, rate, rpm (Rear right motor speed)
# aileron left, cw, deg
# aileron right, cw, deg
# elevator, cw, deg
# rudder, cw, deg
# thrust, pusher, rate, rpm
FR_res = 15500*(FR_pwm - 900.0)/1100.0
RL_res = 15500*(RL_pwm - 900.0)/1100.0
FL_res = 15500*(FL_pwm - 900.0)/1100.0
RR_res = 15500*(RR_pwm - 900.0)/1100.0
aileron1_res = 70*(aileron1_pwm - 1500.0)/500.0
aileron2_res = 70*(aileron2_pwm - 1500.0)/500.0
elevator_res = 70*(elevator_pwm - 1500.0)/500.0
rudder_res = 0.0
thrust_res = 15500*(pusher_pwm - 1000.0)/1000.0
joy.axes.append( round( FR_res, 1) )
joy.axes.append( round( RL_res, 1) )
joy.axes.append( round( FL_res, 1) )
joy.axes.append( round( RR_res, 1) )
joy.axes.append( round( aileron1_res, 1) )
joy.axes.append( round( aileron2_res, 1 ) )
joy.axes.append( round( elevator_res, 1) )
joy.axes.append( round( rudder_res, 1) )
joy.axes.append( round( thrust_res, 1) )
for i in range(4):
if joy.axes[i] < 0:
joy.axes[i] = 0
if joy.axes[8] < 0:
joy.axes[8] = 0
joy_pub.publish(joy)
def __init__(self):
self._light_client = mobile_base.ChestLightClient()
self._joint_states = mobile_base.JointStates()
self._head_client = mobile_base.HeadClient(self._joint_states)
assert self._head_client.wait_for_server(timeout=rospy.Duration(30.0))
# At the start, open Kuri's eyes and point the head up:
self._head_client.pan_and_tilt(
pan=mobile_base.HeadClient.PAN_NEUTRAL,
tilt=mobile_base.HeadClient.TILT_UP,
duration=1.0
)
self._head_client.eyes_to(
radians=mobile_base.HeadClient.EYES_OPEN,
duration=0.5
)
self.vel_pub = rospy.Publisher(
"/mobile_base/commands/velocity", Twist, queue_size=1)
self.joy_sub = rospy.Subscriber(
"/joy", Joy, self.joy_cb, queue_size=1)
self.load_profile()
self.last_msg = Joy()
self.las_vel = Twist()
self.last_played = 0.
self.current_play = None
self.deadman_pressed = False
self.zero_twist_published = False
self.timer = rospy.Timer(rospy.Duration(0.1), self.publish_vel)
def init_ros(self):
try:
rospy.init_node('ps3joy', anonymous=True, disable_signals=True)
except:
print("rosnode init failed")
# My!!!
autorepeat_rate = rospy.get_param("~autorepeat_rate", 10.0) # Hz
# deadzone = 0.05
self.deadzone = rospy.get_param("~deadzone", 10)
self.autorepeat_interval = rospy.Duration.from_sec(
1. / autorepeat_rate) # secs
# self.scale = 1. / (1. - deadzone) / 255.
# self.unscaled_deadzone = 255. * deadzone
self.last_data_time = rospy.get_rostime()
self.data = Joy()
self.data.buttons = [0] * 17
self.data.axes = [0] * 20
self.data_pub = rospy.Publisher('joy/data', Joy, queue_size=10)
print("ps3joy: deadzone=%d, autorepeat_rate=%f" %
(self.deadzone, autorepeat_rate))
#!!!
rospy.Subscriber("joy/set_feedback", sensor_msgs.msg.JoyFeedbackArray,
self.set_feedback)
self.diagnostics = Diagnostics()
self.led_values = [1, 0, 0, 0]
self.rumble_cmd = [0, 255]
self.led_cmd = 2
self.core_down = False
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 __init__(self, name):
""" Constructor """
# rospy.loginfo("%s: JoystickBase constructor", name)
self.name = name
self.joy_msg = Joy()
self.joy_msg.axes = [0] * 12 # 6 pose + 6 twist
self.joy_msg.buttons = [0] * 12 # 6 pose + 6 twist
self.mutual_exclusion = Lock()
# Create publisher
self.pub_map_ack_data = rospy.Publisher("/cola2_control/map_ack_data",
Joy,
queue_size=1)
self.pub_map_ack_ack_teleop = rospy.Publisher(
"/cola2_control/map_ack_ack", String, queue_size=1)
# Create subscriber
rospy.Subscriber("/cola2_control/map_ack_ok",
String,
self.update_ack,
queue_size=4)
rospy.Subscriber("/joy", Joy, self.update_joy, queue_size=4)
# Timer for the publish method
rospy.Timer(rospy.Duration(0.1), self.iterate)
class mapInfo(object):
self.debug=True
def __init__(self,socket):
#original map
self.olpMap='/home/skel/ws2/src/robot_server/scripts/mymap.pgm'
#converted map
self.newMap='/home/skel/ws2/src/robot_server/scripts/mymap.png'
#publisher for starting point
self.pose_pub=rospy.Publisher('initial_pose',PoseWithCovarianceStamped,latch=True)
#pose message
self.pose_msg=PoseWithCovarianceStamped()
#publisher for destination point
self.pose_pubDest=rospy.Publisher('goal_pose',PoseStamped,latch=True)
#message to read position
self.pose_amcl=PoseWithCovarianceStamped()
#pose message
self.pose_msgDest=PoseStamped()
#'Path' message is published in 'sent path'
self.mult_pub = rospy.Publisher('sent_path',Path)
#message 'Path' that holds multiple points
self.mult_msg = Path()
#publisher that will sent message joy to topic Joy
self.joy_pub = rospy.Publisher('joy', Joy)
#messages to be filled with data
self.joy_msg = Joy()
self.joy_msg.buttons.extend([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0])
#matrix with quaternion coordinates
self.quaternion=[0.0,0.0,0.0,0.0]
self.socket=socket
self.covariance = [0] * 36
self.covariance[0]=0.25
self.covariance[7]=0.25
self.covariance[35]=0.06853891945200942
def translate(self, x_target, y_target):
msg = Joy()
vel = 150 #must be small to avoid jerking, and secondly to avoid switching surface
distance_threshold = 0.05
x_error = (x_target - self.x0) * math.cos(
self.yaw0) + (y_target - self.y0) * math.sin(self.yaw0)
y_error = -(x_target - self.x0) * math.sin(
self.yaw0) + (y_target - self.y0) * math.cos(self.yaw0)
print("translate")
print(math.sqrt(x_error**2 + y_error**2))
print(x_target, y_target)
if abs(x_error) > distance_threshold:
if x_error > 0:
vx = 1024 + vel
else:
vx = 1024 - vel
else:
vx = 1024
if abs(y_error) > distance_threshold:
if y_error > 0:
vy = 1024 - vel
else:
vy = 1024 + vel
else:
vy = 1024
msg.buttons = [vx, vy, 1024]
self.cmd_vel_pub.publish(msg)
def __init__(self):
# Drone state
self.state = State()
# Instantiate a setpoints message
self.sp = PositionTarget()
# set the flag to use position setpoints and yaw angle
self.sp.type_mask = int('010111111000', 2)
# LOCAL_NED
self.sp.coordinate_frame = 1
# We will fly at a fixed altitude for now
# Altitude setpoint, [meters]
self.ALT_SP = 3.0
# update the setpoint message with the required altitude
self.sp.position.z = self.ALT_SP
# Instantiate a joystick message
self.joy_msg = Joy()
# initialize
self.joy_msg.axes = [0.0, 0.0, 0.0]
# Step size for position update
self.STEP_SIZE = 2.0
# Fence. We will assume a square fence for now
self.FENCE_LIMIT = 5.0
# A Message for the current local position of the drone
self.local_pos = Point(0.0, 0.0, 0.0)
def __init__(self):
''' connect to midi device and set up ros
'''
pygame.midi.init()
devices = pygame.midi.get_count()
if devices < 1:
rospy.logerr("No MIDI devices detected")
exit(-1)
rospy.loginfo("Found %d MIDI devices" % devices)
input_dev = int(
rospy.get_param("~input_dev", pygame.midi.get_default_input_id))
rospy.loginfo("Using input device %d" % input_dev)
self.controller = pygame.midi.Input(input_dev)
# load in default parameters if not set
if not rospy.has_param('~modes'):
rospack = rospkg.RosPack()
paramlist = rosparam.load_file(
rospack.get_path('korg_nanokontrol') +
'/config/nanokontrol_config.yaml')
for params, ns in paramlist:
rosparam.upload_params(ns, params)
self.modes = rospy.get_param('~modes')
# determine how axis should be interpreted
self.center_axis = rospy.get_param('~center_axis', True)
self.msg = Joy()
self.mode = None
self.pub = rospy.Publisher('joy', Joy, queue_size=10, latch=True)
