def talker():
global pose
pose = PS()
rospy.init_node('talker', anonymous=True)
pub = rospy.Publisher('viconTracker', PS, queue_size=1)
sub = rospy.Subscriber('viconPoseData', Float32[], callback) #data type??
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
pub.publish(pose)
rate.sleep()
def xytheta_to_pose(x, y, theta):
pose = PS()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "map"
pose.pose.position.x = x
pose.pose.position.y = y
quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
pose.pose.orientation.x = quaternion[0]
pose.pose.orientation.y = quaternion[1]
pose.pose.orientation.z = quaternion[2]
pose.pose.orientation.w = quaternion[3]
return pose
def __init__(self):
""" Initializing DDPG """
self.ddpg = DDPG(a_dim=A_DIM,
s_dim=S_DIM,
batch_size=10,
memory_capacity=500)
self.ep_reward = 0.0
self.current_ep = 0
self.current_step = 0
self.current_action = np.array([.0, .0, .0])
self.done = True # if the episode is done
self.var = 5.0
print("Initialized DDPG")
""" Setting communication"""
self.sub1 = rospy.Subscriber('robot_pose',
PA,
self.callback1,
queue_size=1)
self.sub2 = rospy.Subscriber('robot_pose',
PA,
self.callback2,
queue_size=1)
self.pub1 = rospy.Publisher('Robot_5/pose', PS, queue_size=10)
self.pub2 = rospy.Publisher('normalized_state', PC, queue_size=10)
rospy.wait_for_service('airpress_control', timeout=5)
self.target_PS = PS()
self.action_V3 = Vector3()
self.pc = PC()
self.pc.header.frame_id = 'world'
self.state = PA()
self.updated = True # if s_ is updated
self.got_callback1 = False
self.got_callback2 = False
print("Initialized communication")
""" Reading targets """
""" The data should be w.r.t origin by base position """
ends = pickle.load(open('ends.p', 'rb'))
self.r_ends = ends['r_ends']
self.rs_ends = ends['rs_ends']
self.x_offset = X_OFFSET
self.y_offset = Y_OFFSET
self.z_offset = Z_OFFSET
self.scaler = 1 / 0.3
self.sample_target()
print("Read target data")
#self.ddpg.restore_momery()
#self.ddpg.restore_model()
"""
def talker():
pub1 = rospy.Publisher('Robot_5/pose', PS, queue_size=10)
rospy.init_node('target_pub', anonymous=True)
rate = rospy.Rate(50) # 50hz
ps1 = PS()
ps1.pose.position.x = 5.0
ps1.pose.position.y = 5.1
ps1.pose.position.z = 5.2
ps1.pose.orientation.x = 0.4
ps1.pose.orientation.y = 0.5
ps1.pose.orientation.z = 0.6
ps1.pose.orientation.w = 0.7
ps1.header.frame_id = "raw"
while not rospy.is_shutdown():
rospy.loginfo('Publishing target...')
pub1.publish(ps1)
rate.sleep()
def Aruco_marker_detector(self):
# define the ids of marker
# a = 0 # the index of first marker need to detect
self.ids_target[0] = 4
self.ids_target[1] = 7
#set up the real size of the marker
# markerSize = 1.0
# axisLength = 2.0
# setup matrix from imu to cam
self.imu_cam[0][1] = -1.0
self.imu_cam[0][3] = 0.0
self.imu_cam[1][0] = -1.0
self.imu_cam[1][3] = 0.0
self.imu_cam[2][2] = -1.0
self.imu_cam[2][3] = 0.0
self.imu_cam[3][3] = -1.0
# create vector tvec1, tvec2
tvec1 = np.zeros((4, 1), dtype=np.float)
tvec1[3][0] = 1.0
tvec2 = np.zeros((4, 1), dtype=np.float)
while not rospy.is_shutdown():
# print(self.pos[0])
""" Use the build in python library to detect the aruco marker and its coordinates """
img = self.frame.copy()
# Detect the markers in the image
markerCorners, markerIds, rejectedCandidates = cv.aruco.detectMarkers(
img, self.dictionary, parameters=self.parameters)
if np.all(markerIds != None):
ids_marker = True
self.check_marker_detection.publish(ids_marker)
for i in range(0, markerIds.size):
if self.altitude > 3.0:
if markerIds[i][0] == self.ids_target[0]:
# get corner at index i responsible id at index 0
self.corners = markerCorners[i]
markerSize = 0.4
axisLength = 1.0
ret1 = cv.aruco.estimatePoseSingleMarkers(
self.corners, markerSize, self.K,
self.distCoeffs)
rvecs, tvecs = ret1[0][0, 0, :], ret1[1][0, 0, :]
tvec1[0][0] = tvecs[0]
tvec1[1][0] = tvecs[1]
tvec1[2][0] = tvecs[2]
# update altitude of UAV
self.altitude = tvecs[2]
print(self.altitude)
alpha = 5.0
self.beta[0] = abs(
np.rad2deg(np.arctan(tvec1[0][0] /
tvec1[2][0])))
self.beta[1] = abs(
np.rad2deg(np.arctan(tvec1[1][0] /
tvec1[2][0])))
# decide move or decend
check_move = self.check_angle(alpha)
self.check_move_position.publish(check_move)
# marker in the body (UAV frane)
marker_pos = PS()
tvec2 = np.matmul(self.imu_cam, tvec1)
marker_pos.pose.position.x = tvec2[0][0]
marker_pos.pose.position.y = tvec2[1][0]
marker_pos.pose.position.z = tvec2[2][0]
# publish marker in body frame
self.aruco_marker_pos_pub.publish(marker_pos)
## marker in the global frame
fly_pos = PS()
fly_pos.pose.position.x = tvec2[0][0] + self.pos[0]
fly_pos.pose.position.y = tvec2[1][0] + self.pos[1]
fly_pos.pose.position.z = tvec2[2][0] + self.pos[2]
# publish marker in body frame
self.fly_pos_pub.publish(fly_pos)
frame_out = cv.aruco.drawAxis(
img, self.K, self.distCoeffs, rvecs, tvecs,
axisLength)
# self.aruco_marker_pos_pub.publish(marker_pos)
self.aruco_marker_img_pub.publish(
self.bridge.cv2_to_imgmsg(frame_out, "bgr8"))
print(markerSize)
self.rate.sleep()
else:
if markerIds[i][0] == self.ids_target[1]:
# get corner at index i responsible id at index 1
self.corners = markerCorners[i]
markerSize = 0.08
axisLength = 0.5
ret1 = cv.aruco.estimatePoseSingleMarkers(
self.corners, markerSize, self.K,
self.distCoeffs)
rvecs, tvecs = ret1[0][0, 0, :], ret1[1][0, 0, :]
tvec1[0][0] = tvecs[0]
tvec1[1][0] = tvecs[1]
tvec1[2][0] = tvecs[2]
# update altitude of UAV
self.altitude = tvecs[2]
alpha = 3.0
self.beta[0] = abs(
np.rad2deg(np.arctan(tvecs[0] / tvecs[2])))
self.beta[1] = abs(
np.rad2deg(np.arctan(tvecs[1] / tvecs[2])))
# decide move or decend
check_move = self.check_angle(alpha)
self.check_move_position.publish(check_move)
# marker in the body (UAV frane)
marker_pos = PS()
tvec2 = np.matmul(self.imu_cam, tvec1)
marker_pos.pose.position.x = tvec2[0][0]
marker_pos.pose.position.y = tvec2[1][0]
marker_pos.pose.position.z = tvec2[2][0]
# publish marker in body frame
self.aruco_marker_pos_pub.publish(marker_pos)
## marker in the global frame
fly_pos = PS()
fly_pos.pose.position.x = tvec2[0][0] + self.pos[0]
fly_pos.pose.position.y = tvec2[1][0] + self.pos[1]
fly_pos.pose.position.z = tvec2[2][0] + self.pos[2]
# publish marker in body frame
self.fly_pos_pub.publish(fly_pos)
frame_out = cv.aruco.drawAxis(
img, self.K, self.distCoeffs, rvecs, tvecs,
axisLength)
# self.aruco_marker_pos_pub.publish(marker_pos)
self.aruco_marker_img_pub.publish(
self.bridge.cv2_to_imgmsg(frame_out, "bgr8"))
print(markerSize)
self.rate.sleep()
else:
ids_marker = False
self.check_marker_detection.publish(ids_marker)
def talker():
pub1 = rospy.Publisher('Robot_1/pose', PS, queue_size=10)
pub2 = rospy.Publisher('Robot_2/pose', PS, queue_size=10)
pub3 = rospy.Publisher('Robot_3/pose', PS, queue_size=10)
pub4 = rospy.Publisher('Robot_4/pose', PS, queue_size=10)
rospy.init_node('naive_pose_pub', anonymous=True)
rate = rospy.Rate(50) # 50hz
ps1 = PS()
ps2 = PS()
ps3 = PS()
ps4 = PS()
ps1.pose.position.x = 1.0
ps1.pose.position.y = 1.1
ps1.pose.position.z = 1.2
ps1.pose.orientation.x = 0.4
ps1.pose.orientation.y = 0.5
ps1.pose.orientation.z = 0.6
ps1.pose.orientation.w = 0.7
ps1.header.frame_id = "raw"
ps2.pose.position.x = 2.0
ps2.pose.position.y = 2.1
ps2.pose.position.z = 2.2
ps2.pose.orientation.x = 0.4
ps2.pose.orientation.y = 0.5
ps2.pose.orientation.z = 0.6
ps2.pose.orientation.w = 0.7
ps2.header.frame_id = "raw"
ps3.pose.position.x = 3.0
ps3.pose.position.y = 3.1
ps3.pose.position.z = 3.2
ps3.pose.orientation.x = 0.4
ps3.pose.orientation.y = 0.5
ps3.pose.orientation.z = 0.6
ps3.pose.orientation.w = 0.7
ps3.header.frame_id = "raw"
ps4.pose.position.x = 4.0
ps4.pose.position.y = 4.1
ps4.pose.position.z = 4.2
ps4.pose.orientation.x = 0.4
ps4.pose.orientation.y = 0.5
ps4.pose.orientation.z = 0.6
ps4.pose.orientation.w = 0.7
ps4.header.frame_id = "raw"
x = 100000000000.0
y = 100000000000.0
z = 100000000000.0
w = 100000000000.0
while not rospy.is_shutdown():
rospy.loginfo('Publishing naive pose...')
"""
ps1.pose.position.x = x
ps1.pose.position.y = y
ps1.pose.position.z = z
ps1.pose.orientation.x = x
ps1.pose.orientation.y = y
ps1.pose.orientation.z = z
ps1.pose.orientation.w = w
ps1.header.frame_id = "raw"
"""
pub1.publish(ps1)
pub2.publish(ps2)
pub3.publish(ps3)
pub4.publish(ps4)
#x = x*0.999
#y = y*0.999
#z = z*0.999
#w = w*0.999
rate.sleep()
def __init__(self):
""" Initializing DDPG """
self.sim = Sim()
self.ddpg = DDPG(a_dim=A_DIM,
s_dim=S_DIM,
batch_size=10,
memory_capacity=MEMORY_CAPACITY,
gamma=GAMMA) #gamma=0.98
self.ep_reward = 0.0
self.current_ep = 0
self.current_step = 0
self.current_action = np.array([.0, .0, .0])
self.done = True # if the episode is done
self.var = VAR_INIT
self.reward_record = list()
self.ep_record = list()
self.fig = plt.gcf()
self.fig.show()
self.fig.canvas.draw()
print("Initialized DDPG")
""" Reading targets """
""" The data should be w.r.t origin by base position """
self.ends = pickle.load(open('./data/ends.p', 'rb'))
self.x_offset = X_OFFSET
self.y_offset = Y_OFFSET
self.z_offset = Z_OFFSET
self.sample_target()
print("Read target data")
""" Setting communication"""
self.pc = PC()
self.pc.header.frame_id = 'world'
self.pub = rospy.Publisher('normalized_state', PC, queue_size=10)
self.pub1 = rospy.Publisher('state', PC, queue_size=10)
self.sub = rospy.Subscriber('Robot_1/pose',
PS,
self.callback,
queue_size=1)
self.sub1 = rospy.Subscriber('Robot_2/pose',
PS,
self.callback2,
queue_size=1)
rospy.wait_for_service('airpress_control', timeout=5)
self.target_PS = PS()
self.action_V3 = Vector3()
self.updated = False # if s is updated
self.got_target = False
print("Initialized communication")
#self.ddpg.restore_momery()
self.ddpg.restore_model('model3000f')
while not (rospy.is_shutdown()):
if self.current_ep < MAX_EPISODES:
if self.current_step < MAX_EP_STEPS:
self.updated = False
while (not self.updated) & (not rospy.is_shutdown()):
rospy.sleep(0.1)
# rospy.sleep(0.5)
p = self.p.copy()
#p = self.sim.current_pose
s = np.vstack((p, self.target))
#s = s[3:,:]
s = self.normalize_state(s)
norm_a = self.ddpg.choose_action(s.reshape(6)[-S_DIM:])
noise_a = np.random.normal(norm_a, self.var)
action = np.clip(noise_a, -1.0, 1.0)
self.current_action = action * A_BOUND + A_BOUND
# p_ = self.sim.update_pose(self.current_action)
self.action_V3.x, self.action_V3.y, self.action_V3.z \
= self.current_action[0], self.current_action[1], self.current_action[2]
self.run_action(self.action_V3)
rospy.sleep(2.5)
self.updated = False
while (not self.updated) & (not rospy.is_shutdown()):
rospy.sleep(0.1)
p_ = self.p.copy()
s_ = np.vstack((p_, self.target))
#s_ = s_[3:,:]
s_ = self.normalize_state(s_)
self.compute_reward(s[0, :], s_[1, :])
self.current_step += 1
if self.reward > GOT_GOAL:
self.reward += 1.0
self.ep_reward += self.reward
if self.current_ep % 10 == 0:
self.reward_record.append(self.ep_reward /
self.current_step)
self.ep_record.append(self.current_ep)
plt.plot(self.ep_record, self.reward_record)
plt.ylim([-1.2, 0.5])
self.fig.canvas.draw()
self.fig.savefig('learning.png')
print('\n')
print "Target Reached"
print("Normalized action:")
print norm_a
print("Noise action:")
print action
print("Output action:")
print self.current_action
print("Reward: %f" % self.reward)
print(
'Episode:',
self.current_ep,
' Reward: %f' % self.ep_reward,
'Explore: %.3f' % self.var,
)
print('*' * 40)
self.ddpg.save_model()
self.ddpg.save_memory()
self.done = True
self.current_step = 0
self.current_ep += 1
self.sample_target()
self.ep_reward = 0
#self.ddpg.save_memory()
#self.ddpg.save_model()
"""
self.current_action = np.array([.0, .0, .0])
self.action_V3.x, self.action_V3.y, self.action_V3.z \
= self.current_action[0], self.current_action[1], self.current_action[2]
self.run_action(self.action_V3)
"""
else:
self.ep_reward += self.reward
if self.current_step == MAX_EP_STEPS:
if self.current_ep % 10 == 0:
self.reward_record.append(self.ep_reward /
self.current_step)
self.ep_record.append(self.current_ep)
plt.plot(self.ep_record, self.reward_record)
plt.ylim([-1.2, 0.5])
self.fig.canvas.draw()
self.fig.savefig('learning.png')
print('\n')
print "Target Failed"
print("Normalized action:")
print norm_a
print("Noise action:")
print action
print("Output action:")
print self.current_action
print("Reward: %f" % self.reward)
print(
'Episode:',
self.current_ep,
' Reward: %f' % self.ep_reward,
'Explore: %.3f' % self.var,
)
print('*' * 40)
self.ddpg.save_model()
self.ddpg.save_memory()
self.done = False
self.current_step = 0
self.current_ep += 1
self.sample_target()
self.ep_reward = 0
#self.ddpg.save_memory()
#self.ddpg.save_model()
"""
self.current_action = np.array([.0, .0, .0])
self.action_V3.x, self.action_V3.y, self.action_V3.z \
= self.current_action[0], self.current_action[1], self.current_action[2]
self.run_action(self.action_V3)
"""
self.ddpg.store_transition(
s.reshape(6)[-S_DIM:], action, self.reward,
s_.reshape(6)[-S_DIM:])
if self.ddpg.pointer > TRAIN_POINT:
#if (self.current_step % 10 == 0):
#self.var *= VAR_DECAY
#self.var = max(0.0,self.var-1.02/(MAX_EP_STEPS*MAX_EPISODES))
self.ddpg.learn()
self.pub_state(s_)
else:
#p = self.sim.current_pose
self.updated = False
while (not self.updated) & (not rospy.is_shutdown()):
rospy.sleep(0.1)
p = self.p.copy()
self.got_target = False
while (not self.got_target) & (not rospy.is_shutdown()):
rospy.sleep(0.1)
s = np.vstack((p, self.real_target))
#s = s[3:,:]
s = self.normalize_state(s)
norm_a = self.ddpg.choose_action(s.reshape(6)[-S_DIM:])
self.current_action = norm_a * A_BOUND + A_BOUND
print("Normalized action:")
print norm_a
print("Current action:")
print self.current_action
# p_ = self.sim.update_pose(self.current_action)
self.action_V3.x, self.action_V3.y, self.action_V3.z \
= self.current_action[0], self.current_action[1], self.current_action[2]
self.run_action(self.action_V3)
rospy.sleep(2.5)
self.updated = False
while (not self.updated) & (not rospy.is_shutdown()):
rospy.sleep(0.1)
p_ = self.p.copy()
s_ = np.vstack((p_, self.real_target))
#s_ = s_[3:,:]
dist_p = "Distance: " + str(
np.linalg.norm(s_[0, :] - s_[1, :]))
print colored(dist_p, 'red')
s_ = self.normalize_state(s_)
self.compute_reward(s_[0, :], s_[1, :])
print('Explore: %.2f' % self.var, )
print("Reward: %f" % self.reward)
rospy.sleep(1)
#print
print self.ddpg.get_value(
s.reshape(6)[-S_DIM:], norm_a, self.reward.reshape(
(-1, 1)),
s_.reshape(6)[-S_DIM:])
print '\n'
self.pub_state(s_)
if self.reward > GOT_GOAL:
self.done = True
self.current_step = 0
self.current_ep += 1
self.sample_target()
print "Target Reached"
print("Episode %i Ended" % self.current_ep)
print("Reward: %f" % self.reward)
print(
'Episode:',
self.current_ep,
' Reward: %d' % self.ep_reward,
'Explore: %.2f' % self.var,
)
print('*' * 40)
self.ep_reward = 0
# self.ddpg.save_memory()
# self.ddpg.save_model()
"""
self.current_action = np.array([.0, .0, .0])
self.action_V3.x, self.action_V3.y, self.action_V3.z \
= self.current_action[0], self.current_action[1], self.current_action[2]
self.run_action(self.action_V3)
"""
else:
self.done = False
self.current_step += 1
if self.current_step == 2:
print "Target Failed"
print("Reward: %f" % self.reward)
print("Episode %i Ends" % self.current_ep)
print(
'Episode:',
self.current_ep,
' Reward: %d' % self.ep_reward,
'Explore: %.2f' % self.var,
)
print('*' * 40)
self.current_step = 0
self.current_ep += 1
self.sample_target()
self.ep_reward = 0
# self.ddpg.save_memory()
# self.ddpg.save_model()
"""
def marker_pose(self):
# Get device product line for setting a supporting resolution
pipeline_wrapper = rs.pipeline_wrapper(self.pipeline)
pipeline_profile = self.config.resolve(pipeline_wrapper)
device = pipeline_profile.get_device()
device_product_line = str(device.get_info(rs.camera_info.product_line))
self.config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8,
30)
self.pipeline.start(self.config)
# self.cap.set(3, 1280)
# self.cap.set(4, 720)
# set dictionary size depending on the aruco marker selected
self.param.adaptiveThreshConstant = 7
# setup matrix from imu to cam
self.imu_cam[0][1] = -1.0
self.imu_cam[0][3] = 0.06
self.imu_cam[1][0] = -1.0
self.imu_cam[1][3] = 0.04
self.imu_cam[2][2] = -1.0
self.imu_cam[2][3] = -0.08
self.imu_cam[3][3] = 1.0
# create vector tvec1, tvec2
tvec1 = np.zeros((4, 1), dtype=np.float)
tvec1[3][0] = 1.0
tvec2 = np.zeros((4, 1), dtype=np.float)
# tvec2[3][0] = 1.0
# define the ids of marker
# a = 0 # the index of first marker need to detect
self.ids_target[0] = 11
self.ids_target[1] = 15
# markerLength=0.4
while not rospy.is_shutdown():
frames = self.pipeline.wait_for_frames()
# ret, frame = self.cap.read()
color_frame = frames.get_color_frame()
if not color_frame:
continue
frame = np.asanyarray(color_frame.get_data())
# operations on the frame
gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
# lists of ids and the corners belonging to each id
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, self.dict, parameters=self.param)
if np.all(ids is not None):
for i in range(0, ids.size):
if ids[i][0] == self.ids_target[0]:
self.corners = corners[i]
markerLength = 0.4
ret1 = aruco.estimatePoseSingleMarkers(
corners=self.corners,
markerLength=markerLength,
cameraMatrix=self.mtx,
distCoeffs=self.dist)
rvec, tvec = ret1[0][0, 0, :], ret1[1][0, 0, :]
(rvec - tvec).any(
) # get rid of that nasty numpy value array error
tvec1[0][0] = tvec[0]
tvec1[1][0] = tvec[1]
tvec1[2][0] = tvec[2]
## marker in the body (UAV frane)
fly_pos = PS()
tvec2 = np.matmul(self.imu_cam, tvec1)
fly_pos.pose.position.x = -tvec2[0][0]
fly_pos.pose.position.y = -tvec2[1][0]
fly_pos.pose.position.z = -tvec2[2][0]
# publish marker in body frame
self.fly_pos_pub.publish(fly_pos)
# if (self.local_pos[3] > -0.1 and self.local_pos[3] < 0.1):
marker_pos = PS()
tvec2 = np.matmul(self.imu_cam, tvec1)
marker_pos.pose.position.x = tvec2[0][0]
marker_pos.pose.position.y = tvec2[1][0]
marker_pos.pose.position.z = tvec2[2][0]
self.aruco_marker_pos_pub.publish(marker_pos)
# -- Draw the detected marker and put a reference frame over it
# aruco.drawDetectedMarkers(frame, corners, ids)
aruco.drawAxis(frame, self.mtx, self.dist, rvec, tvec,
0.1)
str_position0 = "Marker Position in Camera frame: x=%f y=%f z=%f" % (
tvec2[0][0], tvec2[1][0], tvec2[2][0])
cv2.putText(frame, str_position0, (0, 50), self.font,
0.7, (0, 255, 0), 1, cv2.LINE_AA)
self.rate.sleep()
cv2.imshow("frame", frame)
cv2.waitKey(1)
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import PoseStamped as PS
import time
import os
import json
rospy.init_node('goal_publisher')
data = {}
goal = PS()
seq = 0
print os.getcwd()
def pub_gloal():
if not os.path.exists('goal.json'):
print 'No Goal Got yet!'
listener()
return
pub = rospy.Publisher('~/move_base_simple/goal', PS, queue_size=1)
with open('goal.json', 'r') as a:
data = json.loads(a.read())
t = time.time()
goal.header.stamp.secs = int(t)
goal.header.stamp.nsecs = int((t - int(t)) * 10**9)
goal.header.frame_id = 'map'
goal.pose.position.x = data['x']
goal.pose.position.y = data['y']
def marker_pose(self):
# Get device product line for setting a supporting resolution
pipeline_wrapper = rs.pipeline_wrapper(self.pipeline)
pipeline_profile = self.config.resolve(pipeline_wrapper)
device = pipeline_profile.get_device()
device_product_line = str(device.get_info(rs.camera_info.product_line))
self.config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8,
15)
self.pipeline.start(self.config)
# self.cap.set(3, 1280)
# self.cap.set(4, 720)
# set dictionary size depending on the aruco marker selected
self.param.adaptiveThreshConstant = 7
# setup matrix from imu to cam
self.imu_cam[0][1] = -1.0
self.imu_cam[0][3] = 0.06
self.imu_cam[1][0] = -1.0
self.imu_cam[1][3] = 0.04
self.imu_cam[2][2] = -1.0
self.imu_cam[2][3] = -0.08
self.imu_cam[3][3] = 1.0
# create vector tvec1, tvec2
tvec1 = np.zeros((4, 1), dtype=np.float)
tvec1[3][0] = 1.0
tvec2 = np.zeros((4, 1), dtype=np.float)
# tvec2[3][0] = 1.0
# define the ids of marker
# a = 0 # the index of first marker need to detect
self.ids_target[0] = 11
self.ids_target[1] = 15
# markerLength=0.4
while not rospy.is_shutdown():
frames = self.pipeline.wait_for_frames()
# ret, frame = self.cap.read()
color_frame = frames.get_color_frame()
if not color_frame:
continue
frame = np.asanyarray(color_frame.get_data())
# operations on the frame
gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
# lists of ids and the corners belonging to each id
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, self.dict, parameters=self.param)
if np.all(ids is not None):
ids_marker = True
self.check_marker_detection.publish(ids_marker)
for i in range(0, ids.size):
if self.altitude > 3.0:
if ids[i][0] == self.ids_target[0]:
self.corners = corners[i]
markerLength = 0.4
ret1 = aruco.estimatePoseSingleMarkers(
corners=self.corners,
markerLength=markerLength,
cameraMatrix=self.mtx,
distCoeffs=self.dist)
rvec, tvec = ret1[0][0, 0, :], ret1[1][0, 0, :]
# -- Draw the detected marker and put a reference frame over it
aruco.drawDetectedMarkers(frame, corners, ids)
aruco.drawAxis(frame, self.mtx, self.dist, rvec,
tvec, 0.1)
str_position0 = "Marker Position in Camera frame: x=%f y=%f z=%f" % (
tvec[0], tvec[1], tvec[2])
cv2.putText(frame, str_position0, (0, 50),
self.font, 0.7, (0, 255, 0), 1,
cv2.LINE_AA)
(rvec - tvec).any(
) # get rid of that nasty numpy value array error
tvec1[0][0] = tvec[0]
tvec1[1][0] = tvec[1]
tvec1[2][0] = tvec[2]
self.altitude = tvec[2]
alpha = 5.0
self.beta[0] = abs(
np.rad2deg(np.arctan(tvec[0] / tvec[2])))
self.beta[1] = abs(
np.rad2deg(np.arctan(tvec[1] / tvec[2])))
# decide move or decend
check_move = self.check_angle(alpha)
self.check_move_position.publish(check_move)
# marker in the body (UAV frane)
marker_pos = PS()
tvec2 = np.matmul(self.imu_cam, tvec1)
marker_pos.pose.position.x = tvec2[0][0]
marker_pos.pose.position.y = tvec2[1][0]
marker_pos.pose.position.z = tvec2[2][0]
# publish marker in body frame
self.aruco_marker_pos_pub.publish(marker_pos)
# if (self.local_pos[3] > -0.1 and self.local_pos[3] < 0.1):
## marker in the global frame
fly_pos = PS()
fly_pos.pose.position.x = tvec2[0][
0] + self.local_pos[0]
fly_pos.pose.position.y = tvec2[1][
0] + self.local_pos[1]
fly_pos.pose.position.z = tvec2[2][
0] + self.local_pos[2]
# publish marker in body frame
self.fly_pos_pub.publish(fly_pos)
self.rate.sleep()
# publish target position in world frame
# target_pos = PS()
# target_pos.pose.position.x = self.local_pos[0] + tvec2[0][0]
# target_pos.pose.position.y = self.local_pos[1] + tvec2[1][0]
# self.target_position.publish(target_pos)
# if (self.altitude < 4):
# a = 1
# markerLength = 0.08
# break
# check_err = np.linalg.norm([tvec2[0][0], tvec2[1][0]])
# self.check_error_pos.publish(check_err)
# self.rate.sleep()
#str_position0 = "Marker Position in Camera frame: x=%f y=%f " % (tvec2[0][0], tvec2[1][0])
# cv2.putText(frame, str_position0, (0, 50), self.font, 1, (0, 255, 0), 2, cv2.LINE_AA)
# else:
# # # change form
# rotMat = tr.eulerAnglesToRotationMatrix([0, 0, self.local_pos[3]])
# rotMat = np.matmul(rotMat, self.imu_cam)
# # rotMat = rotMat[0:3, 0:3]
# tvec2 = np.matmul(rotMat, tvec1)
# # publish marker position in uav frame
# marker_pos = PS()
# marker_pos.pose.position.x = tvec2[0][0]
# marker_pos.pose.position.y = tvec2[1][0]
# marker_pos.pose.position.z = tvec2[2][0]
# self.aruco_marker_pos_pub.publish(marker_pos)
# # publish target position in world frame
# target_pos = PS()
# target_pos.pose.position.x = self.local_pos[0] + tvec2[0][0]
# target_pos.pose.position.y = self.local_pos[1] + tvec2[1][0]
# self.target_position.publish(target_pos)
# #str_position0 = "Marker Position in Camera frame: x=%f y=%f " % (tvec2[0][0], tvec2[1][0])
# #cv2.putText(frame, str_position0, (0, 50), self.font, 1, (0, 255, 0), 2, cv2.LINE_AA)
# check_err = np.linalg.norm([tvec2[0][0], tvec2[1][0]])
# self.check_error_pos.publish(check_err)
# self.rate.sleep()
else:
if ids[i][0] == self.ids_target[1]:
# get corner at index i responsible id at index 1
self.corners = corners[i]
markerLength = 0.08
ret1 = aruco.estimatePoseSingleMarkers(
corners=self.corners,
markerLength=markerLength,
cameraMatrix=self.mtx,
distCoeffs=self.dist)
rvec, tvec = ret1[0][0, 0, :], ret1[1][0, 0, :]
# -- Draw the detected marker and put a reference frame over it
aruco.drawDetectedMarkers(frame, corners, ids)
aruco.drawAxis(frame, self.mtx, self.dist, rvec,
tvec, 0.1)
str_position0 = "Marker Position in Camera frame: x=%f y=%f z=%f" % (
tvec[0], tvec[1], tvec[2])
cv2.putText(frame, str_position0, (0, 50),
self.font, 0.7, (0, 255, 0), 1,
cv2.LINE_AA)
(rvec - tvec).any(
) # get rid of that nasty numpy value array error
tvec1[0][0] = tvec[0]
tvec1[1][0] = tvec[1]
tvec1[2][0] = tvec[2]
self.altitude = tvec[2]
alpha = 3.0
self.beta[0] = abs(
np.rad2deg(np.arctan(tvec[0] / tvec[2])))
self.beta[1] = abs(
np.rad2deg(np.arctan(tvec[1] / tvec[2])))
# decide move or decend
check_move = self.check_angle(alpha)
self.check_move_position.publish(check_move)
# marker in the body (UAV frane)
marker_pos = PS()
tvec2 = np.matmul(self.imu_cam, tvec1)
marker_pos.pose.position.x = tvec2[0][0]
marker_pos.pose.position.y = tvec2[1][0]
marker_pos.pose.position.z = tvec2[2][0]
# publish marker in body frame
self.aruco_marker_pos_pub.publish(marker_pos)
# if (self.local_pos[3] > -0.1 and self.local_pos[3] < 0.1):
## marker in the global frame
fly_pos = PS()
fly_pos.pose.position.x = tvec2[0][
0] + self.local_pos[0]
fly_pos.pose.position.y = tvec2[1][
0] + self.local_pos[1]
fly_pos.pose.position.z = tvec2[2][
0] + self.local_pos[2]
# publish marker in body frame
self.fly_pos_pub.publish(fly_pos)
self.rate.sleep()
else:
ids_marker = False
self.check_marker_detection.publish(ids_marker)
cv2.imshow("frame", frame)
cv2.waitKey(1)
def __init__(self):
""" Initializing DDPG """
self.sim = Sim()
self.pfn = PFN(a_dim=A_DIM,
s_dim=S_DIM,
batch_size=8,
memory_capacity=MEMORY_NUM,
lr=0.001,
bound=A_BOUND)
self.ep_reward = 0.0
self.current_action = np.array([.0, .0, .0])
self.done = True # if the episode is done
self.reward_record = list()
self.ep_record = list()
self.fig = plt.gcf()
self.fig.show()
self.fig.canvas.draw()
print("Initialized PFN")
""" Setting communication"""
self.pc = PC()
self.pc.header.frame_id = 'world'
self.pub = rospy.Publisher('normalized_state', PC, queue_size=10)
self.pub1 = rospy.Publisher('state', PC, queue_size=10)
self.sub = rospy.Subscriber('Robot_1/pose',
PS,
self.callback,
queue_size=1)
rospy.wait_for_service('airpress_control', timeout=5)
self.target_PS = PS()
self.action_V3 = Vector3()
self.updated = False # if s is updated
self.got_target = False
print("Initialized communication")
""" Reading targets """
""" The data should be w.r.t origin by base position """
self.ends = pickle.load(open('./data/targets.p', 'rb'))
self.x_offset = X_OFFSET
self.y_offset = Y_OFFSET
self.z_offset = Z_OFFSET
self.sample_target()
print("Read target data")
#self.pfn.restore_momery()
self.pfn.restore_model('model_pfn')
memory_ep = np.ones((MAX_EP_STEPS, 3 + 3 + 1 + 1)) * -100
self.current_ep = 0
self.current_step = 0
while not (rospy.is_shutdown()):
self.updated = False
while (not self.updated) & (not rospy.is_shutdown()):
rospy.sleep(0.1)
real_target = self.real_target.copy()
s = self.normalize_state(real_target)
action, act_var = self.pfn.choose_action2(s)
self.action_V3.x, self.action_V3.y, self.action_V3.z \
= action[0], action[1], action[2]
self.run_action(self.action_V3)
print '\n'
#rospy.sleep(1.0)
'''
def __init__(self):
""" Initializing DDPG """
self.ddpg = DDPG(a_dim=A_DIM,
s_dim=S_DIM,
batch_size=10,
memory_capacity=500)
self.ep_reward = 0.0
self.current_ep = 0
self.current_step = 0
self.current_action = np.array([.0, .0, .0])
self.done = True # if the episode is done
self.var = 5.0
print("Initialized DDPG")
""" Setting communication"""
self.sub = rospy.Subscriber('robot_pose',
PA,
self.callback,
queue_size=1)
self.pub = rospy.Publisher('normalized_state', PC, queue_size=10)
rospy.wait_for_service('airpress_control', timeout=5)
self.target_PS = PS()
self.action_V3 = Vector3()
self.pc = PC()
self.pc.header.frame_id = 'world'
self.state = PA()
self.updated = False # if s is updated
self.got_callback1 = False
self.got_callback2 = False
print("Initialized communication")
""" Reading targets """
""" The data should be w.r.t origin by base position """
self.ends = pickle.load(open('./data/ends.p', 'rb'))
self.x_offset = X_OFFSET
self.y_offset = Y_OFFSET
self.z_offset = Z_OFFSET
self.scaler = 1 / 0.3
self.sample_target()
print("Read target data")
#self.ddpg.restore_momery()
#self.ddpg.restore_model()
while not (rospy.is_shutdown()):
if self.current_ep < MAX_EPISODES:
if self.current_step < MAX_EP_STEPS:
while (not self.updated) & (not rospy.is_shutdown()):
rospy.sleep(0.1)
s = self.s.copy()
delta_a = self.ddpg.choose_action(s) * A_BOUND
print delta_a / A_BOUND
print("Delta action:")
print delta_a
delta_a = np.random.normal(delta_a, self.var)
print("Noise delta action: ")
print delta_a
self.current_action += delta_a
self.current_action = np.clip(self.current_action, 0, 25)
self.action_V3.x, self.action_V3.y, self.action_V3.z \
= self.current_action[0], self.current_action[1], self.current_action[2]
self.run_action(self.action_V3)
rospy.sleep(2.5)
print "Current action:"
print self.current_action
self.updated = False
while (not self.updated) & (not rospy.is_shutdown()):
rospy.sleep(0.1)
s_ = self.s.copy()
self.compute_reward(self.end, self.n_t)
action = delta_a / A_BOUND
print action
self.ddpg.store_transition(s, action, self.reward, s_)
# print("Experience stored")
if self.ddpg.pointer > TRAIN_POINT:
if (self.current_step % 2 == 0):
self.var *= 0.992
self.ddpg.learn()
self.current_step += 1
self.ep_reward += self.reward
if self.reward > GOT_GOAL:
self.done = True
self.current_step = 0
self.current_ep += 1
self.sample_target()
print "Target Reached"
print("Episode %i Ended" % self.current_ep)
print(
'Episode:',
self.current_ep,
' Reward: %d' % self.ep_reward,
'Explore: %.2f' % self.var,
)
print('*' * 40)
self.ep_reward = 0
self.ddpg.save_memory()
self.ddpg.save_model()
"""
self.current_action = np.array([.0, .0, .0])
self.action_V3.x, self.action_V3.y, self.action_V3.z \
= self.current_action[0], self.current_action[1], self.current_action[2]
self.run_action(self.action_V3)
"""
else:
self.done = False
if self.current_step == MAX_EP_STEPS:
print "Target Failed"
print("Episode %i Ends" % self.current_ep)
print(
'Episode:',
self.current_ep,
' Reward: %d' % self.ep_reward,
'Explore: %.2f' % self.var,
)
print('*' * 40)
self.current_step = 0
self.current_ep += 1
self.sample_target()
self.ep_reward = 0
self.ddpg.save_memory()
self.ddpg.save_model()
"""
self.current_action = np.array([.0, .0, .0])
self.action_V3.x, self.action_V3.y, self.action_V3.z \
= self.current_action[0], self.current_action[1], self.current_action[2]
self.run_action(self.action_V3)
"""
self.updated = False
print('\n')
def Aruco_marker_detector(self):
# define the ids of marker
# a = 0 # the index of first marker need to detect
self.ids_target[0] = 4
self.ids_target[1] = 7
#set up the real size of the marker
# markerSize = 1.0
# axisLength = 2.0
# setup matrix from imu to cam
self.imu_cam[0][1] = -1.0
self.imu_cam[0][3] = 0.0
self.imu_cam[1][0] = -1.0
self.imu_cam[1][3] = 0.0
self.imu_cam[2][2] = -1.0
self.imu_cam[2][3] = 0.0
self.imu_cam[3][3] = 1.0
# create vector tvec1, tvec2
tvec1 = np.zeros((4, 1), dtype=np.float)
tvec1[3][0] = 1.0
tvec2 = np.zeros((4, 1), dtype=np.float)
while not rospy.is_shutdown():
# print(self.pos[0])
""" Use the build in python library to detect the aruco marker and its coordinates """
img = self.frame.copy()
# Detect the markers in the image
markerCorners, markerIds, rejectedCandidates = cv.aruco.detectMarkers(
img, self.dictionary, parameters=self.parameters)
if np.all(markerIds != None):
for i in range(0, markerIds.size):
if markerIds[i][0] == self.ids_target[0]:
# get corner at index i responsible id at index 0
self.corners = markerCorners[i]
markerSize = 1.0
axisLength = 2.0
ret1 = cv.aruco.estimatePoseSingleMarkers(
self.corners, markerSize, self.K, self.distCoeffs)
rvecs, tvecs = ret1[0][0, 0, :], ret1[1][0, 0, :]
tvec1[0][0] = tvecs[0]
tvec1[1][0] = tvecs[1]
tvec1[2][0] = tvecs[2]
## marker in the body (UAV frane)
fly_pos = PS()
tvec2 = np.matmul(self.imu_cam, tvec1)
fly_pos.pose.position.x = -tvec2[0][0]
fly_pos.pose.position.y = -tvec2[1][0]
fly_pos.pose.position.z = -tvec2[2][0]
# publish marker in body frame
self.fly_pos_pub.publish(fly_pos)
# marker in the body (UAV frane)
marker_pos = PS()
tvec2 = np.matmul(self.imu_cam, tvec1)
marker_pos.pose.position.x = tvec2[0][0]
marker_pos.pose.position.y = tvec2[1][0]
marker_pos.pose.position.z = tvec2[2][0]
# publish marker in body frame
self.aruco_marker_pos_pub.publish(marker_pos)
# self.aruco_marker_pos_pub.publish(marker_pos)
frame_out = cv.aruco.drawAxis(img, self.K,
self.distCoeffs, rvecs,
tvecs, axisLength)
# self.aruco_marker_pos_pub.publish(marker_pos)
self.aruco_marker_img_pub.publish(
self.bridge.cv2_to_imgmsg(frame_out, "bgr8"))
# print(markerSize)
self.rate.sleep()
