path_list = update_list(path_list, result[1], i)
n = len(path_list)
return path_list
def update_robot_pos(data):
data2 = data.pose[17]
orientation_list = [data2.orientation.x, data2.orientation.y, data2.orientation.z, data2.orientation.w]
(roll, pitch, yaw) = euler_from_quaternion (orientation_list)
global car
car = [data2.position.x, data2.position.y, yaw + pi]
#dummy
motor_cmd = Float64MultiArray()
suspension_cmd = Float64()
car = [0, 0, 0]
#fixed parameters
obstacle_list = [[4,1.5], [5.5,0.7], [5.5,2.3], [7,1.5]]
#parameters to adjust
spd = 7
[p_gain, i_gain, d_gain] = [7, 0, 0]
motor_cmd_topic_name = '/toy_car2/joint_vel_controller/command'
suspension_topic_front_left = '/toy_car2/front_left_suspension_controller/command'
suspension_topic_front_right = '/toy_car2/front_right_suspension_controller/command'
suspension_topic_rear_left = '/toy_car2/rear_left_suspension_controller/command'
suspension_topic_rear_right = '/toy_car2/rear_right_suspension_controller/command'
rospy.sleep(2)
if __name__=="__main__":
# Setting-up processes
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('panda_demo', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("panda_arm")
gripper_publisher = rospy.Publisher('/franka/gripper_position_controller/command', Float64MultiArray, queue_size=1)
gripper_msg = Float64MultiArray()
gripper_msg.layout.dim = [MultiArrayDimension('', 2, 1)]
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory,queue_size=20)
rospy.sleep(2)
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.153745
p.pose.position.y = -0.301298
p.pose.position.z = 0.
p.pose.orientation.x = 0.6335811
p.pose.orientation.y = 0
def run(self):
'''
Runs ROS node - computes PID algorithms for cascade attitude control.
'''
while (rospy.get_time() == 0) and (not rospy.is_shutdown()):
print 'Waiting for clock server to start'
print 'Received first clock message'
while (not self.start_flag) and (not rospy.is_shutdown()):
print "Waiting for the first measurement."
rospy.sleep(0.5)
print "Starting attitude control."
self.t_old = rospy.Time.now()
clock_old = self.clock
#self.t_old = datetime.now()
self.count = 0
self.loop_count = 0
while not rospy.is_shutdown():
#self.ros_rate.sleep()
while (not self.start_flag) and (not rospy.is_shutdown()):
print "Waiting for the first measurement."
rospy.sleep(0.5)
rospy.sleep(1.0/float(self.rate))
self.euler_sp_filt.x = simple_filters.filterPT1(self.euler_sp_old.x,
self.euler_sp.x, self.roll_reference_prefilter_T, self.Ts,
self.roll_reference_prefilter_K)
self.euler_sp_filt.y = simple_filters.filterPT1(self.euler_sp_old.y,
self.euler_sp.y, self.pitch_reference_prefilter_T, self.Ts,
self.pitch_reference_prefilter_K)
self.euler_sp_filt.z = self.euler_sp.z
#self.euler_sp.z = simple_filters.filterPT1(self.euler_sp_old.z, self.euler_sp.z, 0.2, self.Ts, 1.0)
self.euler_sp_old = copy.deepcopy(self.euler_sp_filt)
clock_now = self.clock
dt_clk = (clock_now.clock - clock_old.clock).to_sec()
clock_old = clock_now
if dt_clk > (1.0 / self.rate + 0.005):
self.count += 1
print self.count, ' - ', dt_clk
if dt_clk < (1.0 / self.rate - 0.005):
self.count += 1
print self.count, ' - ', dt_clk
if dt_clk < 0.005:
dt_clk = 0.01
# Roll
roll_rate_sv = self.pid_roll.compute(self.euler_sp_filt.x, self.euler_mv.x, dt_clk)
# roll rate pid compute
roll_rate_output = self.pid_roll_rate.compute(roll_rate_sv, self.euler_rate_mv.x, dt_clk) + self.roll_rate_output_trim
# Pitch
pitch_rate_sv = self.pid_pitch.compute(self.euler_sp_filt.y, self.euler_mv.y, dt_clk)
# pitch rate pid compute
pitch_rate_output = self.pid_pitch_rate.compute(pitch_rate_sv, self.euler_rate_mv.y, dt_clk) + self.pitch_rate_output_trim
# Yaw
yaw_rate_sv = self.pid_yaw.compute(self.euler_sp_filt.z, self.euler_mv.z, dt_clk)
# yaw rate pid compute
yaw_rate_output = self.pid_yaw_rate.compute(yaw_rate_sv, self.euler_rate_mv.z, dt_clk)
# VPC stuff
vpc_roll_output = -self.pid_vpc_roll.compute(0.0, roll_rate_output, dt_clk)
# Due to some wiring errors we set output to +, should be -
vpc_pitch_output = -self.pid_vpc_pitch.compute(0.0, pitch_rate_output, dt_clk)
# Publish mass position
#mass0_command_msg = Float64()
#mass0_command_msg.data = dx_pitch
#mass2_command_msg = Float64()
#mass2_command_msg.data = -dx_pitch
#mass1_command_msg = Float64()
#mass1_command_msg.data = -dy_roll
#mass3_command_msg = Float64()
#mass3_command_msg.data = dy_roll
#self.pub_mass0.publish(mass0_command_msg)
#self.pub_mass1.publish(mass1_command_msg)
#self.pub_mass2.publish(mass2_command_msg)
#self.pub_mass3.publish(mass3_command_msg)
# Publish attitude
attitude_output = Float64MultiArray()
attitude_output.data = [roll_rate_output, pitch_rate_output, \
yaw_rate_output, vpc_roll_output, vpc_pitch_output]
self.attitude_pub.publish(attitude_output)
# Publish PID data - could be usefule for tuning
self.pub_pid_roll.publish(self.pid_roll.create_msg())
self.pub_pid_roll_rate.publish(self.pid_roll_rate.create_msg())
self.pub_pid_pitch.publish(self.pid_pitch.create_msg())
self.pub_pid_pitch_rate.publish(self.pid_pitch_rate.create_msg())
self.pub_pid_yaw.publish(self.pid_yaw.create_msg())
self.pub_pid_yaw_rate.publish(self.pid_yaw_rate.create_msg())
# Publish VPC pid data
self.pub_pid_vpc_roll.publish(self.pid_vpc_roll.create_msg())
self.pub_pid_vpc_pitch.publish(self.pid_vpc_pitch.create_msg())
#Fill Data Log msg
visionData = PPCVisionData()
visionData.eu = list(eu)
visionData.ev = list(ev)
visionData.u = list(u)
visionData.v = list(v)
visionData.Ucmds = list(Ucmds)
visionData.rhol_u = list(-Ml_u*rhol_u)
visionData.rhol_v = list(-Ml_v*rhol_v)
visionData.rhou_u = list(Mu_u*rhou_u)
visionData.rhou_v = list(Mu_v*rhou_v)
#Fill CMD Msg
cmd = Float64MultiArray()
cmd.layout.dim = [MultiArrayDimension()]
cmd.layout.dim[0].size = 7
cmd.layout.dim[0].stride = 7
cmd.layout.data_offset = 0
cmd.data = list (float (qvelt) for qvelt in qv)
#print cmd.data
pub_vel.publish (cmd)
pub_data.publish (visionData)
rate_it.sleep()
rospy.spin()
except rospy.ROSInterruptException: pass
twist = Twist()
cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
location_check_topic = '/location_check'
location_check_pub = rospy.Publisher(location_check_topic,
Float64MultiArray,
queue_size=1)
boundary_size_topic = "/boundary_size"
boundary_size_pub = rospy.Publisher(boundary_size_topic,
Float64MultiArray,
queue_size=10)
front_boundary = 0.20
width_boundary = 0.19
side_margin = 0.5
boundary_size = Float64MultiArray()
boundary_dodge = Float64MultiArray()
boundary_rotation = Float64MultiArray()
boundary_size.data = [front_boundary, width_boundary]
boundary_dodge.data = [front_boundary, width_boundary + side_margin]
boundary_rotation.data = [0, width_boundary + 0.35]
side_flag = ''
robot_z = 0
scan_rectangle_R = []
scan_rectangle_F = []
scan_rectangle_L = []
def quaternion_to_euler_angle(msg):
def callback(data):
bridge = CvBridge()
# road
color_min = np.array([0, 15, 20], np.uint8)
color_max = np.array([60, 240, 200], np.uint8)
# color_min = np.array([25, 100, 100],np.uint8)
# color_max = np.array([35, 255, 255],np.uint8)
# home
obj_color_min = np.array([150, 150, 100], np.uint8)
obj_color_max = np.array([180, 240, 200], np.uint8)
bgr_cal = []
# try:
# pub_size = rospy.Publisher('/obj_size', Float64, queue_size=1)
try:
with open("/home/evan/catkin_ws/src/cozmo_run/src/wb.txt", "r") as f:
bgr_cal = f.read().split()
# print(type(bgr_cal[0]))
for i in xrange(len(bgr_cal)):
bgr_cal[i] = float(bgr_cal[i])
# print(type(bgr_cal[0]))
except:
# bgr_cal=[1,1,1]
print('wb_error')
pass
try:
pub_head = rospy.Publisher('/head_angle', Float64, queue_size=1)
head_angle = -5
pub_head.publish(head_angle)
frame_raw = bridge.imgmsg_to_cv2(data, "bgr8")
frame_raw = cv2.flip(frame_raw, 1)
height, width = frame_raw.shape[:2]
frame_cpy = frame_raw.copy()
b, g, r = cv2.split(frame_cpy)
cv2.convertScaleAbs(b, b, bgr_cal[0])
cv2.convertScaleAbs(g, g, bgr_cal[1])
cv2.convertScaleAbs(r, r, bgr_cal[2])
frame_cpy = cv2.merge((b, g, r))
# Original
# frame_lower = frame_cpy[200:239, 30:305]
# test
# Frame cropping setting
frame_lower = frame_cpy[120:160, 15:305]
# obj_frame = frame_cpy[140:160,30:290]
frame_higher = frame_cpy[90:110, 30:290]
# obj_frame = frame_cpy[100:120, 40:280]
obj_frame = frame_cpy[110:120, 40:280]
frame_cpy = cv2.flip(frame_cpy, 1)
kernel1 = np.ones((5, 5), np.float32) / 25
kernel2 = np.ones((3, 3), np.float32) / 9
# whole picture frame setting
frame_hsv = cv2.cvtColor(frame_cpy, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(frame_hsv, color_min, color_max)
res = cv2.bitwise_and(frame_hsv, frame_hsv, mask=mask)
res = cv2.erode(res, kernel1, iterations=1)
res = cv2.dilate(res, kernel1, iterations=1)
gray = cv2.cvtColor(res, cv2.COLOR_HSV2BGR)
gray = cv2.cvtColor(gray, cv2.COLOR_BGR2GRAY)
canny = cv2.Canny(gray, 100, 200)
_, contours, _ = cv2.findContours(gray, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
# closer distance frame setting
frame_hsv_lower = cv2.cvtColor(frame_lower, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(frame_hsv_lower, color_min, color_max)
res_lower = cv2.bitwise_and(frame_hsv_lower,
frame_hsv_lower,
mask=mask)
# res_lower = cv2.erode(res_lower, kernel1, iterations=1)
res_lower = cv2.dilate(res_lower, kernel1, iterations=1)
gray_lower = cv2.cvtColor(res_lower, cv2.COLOR_HSV2BGR)
gray_lower = cv2.cvtColor(gray_lower, cv2.COLOR_BGR2GRAY)
canny_lower = cv2.Canny(gray_lower, 100, 200)
_, contours_lower, _ = cv2.findContours(gray_lower, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
# obj area frame setting
obj_frame_hsv = cv2.cvtColor(obj_frame, cv2.COLOR_BGR2HSV)
obj_mask = cv2.inRange(obj_frame_hsv, obj_color_min, obj_color_max)
obj_res = cv2.bitwise_and(obj_frame_hsv, obj_frame_hsv, mask=obj_mask)
obj_res = cv2.erode(obj_res, kernel2, iterations=1)
obj_res = cv2.dilate(obj_res, kernel2, iterations=1)
obj_gray = cv2.cvtColor(obj_res, cv2.COLOR_HSV2BGR)
obj_gray = cv2.cvtColor(obj_gray, cv2.COLOR_BGR2GRAY)
_, obj_contours, _ = cv2.findContours(obj_gray, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
# canny = cv2.Canny(gray, 100, 200)
# longer distance frame setting
frame_hsv_h = cv2.cvtColor(frame_higher, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(frame_hsv_h, color_min, color_max)
res_h = cv2.bitwise_and(frame_hsv_h, frame_hsv_h, mask=mask)
res_h = cv2.erode(res_h, kernel2, iterations=1)
res_h = cv2.dilate(res_h, kernel2, iterations=1)
gray_h = cv2.cvtColor(res_h, cv2.COLOR_HSV2BGR)
gray_h = cv2.cvtColor(gray_h, cv2.COLOR_BGR2GRAY)
_, contours_h, _ = cv2.findContours(gray_h, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
# lower area publish
a = Float64MultiArray()
if len(contours_lower) != 0:
c = max(contours_lower, key=cv2.contourArea)
# cnt = c
# print(cnt)
area = cv2.contourArea(c)
M = cv2.moments(c)
if area != 0:
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
else:
cx = default_x
cy = default_y
if area > min_track_size:
# a.data = [area,cx, cy]
a.data.append(0)
a.data.append(area)
a.data.append(cx)
a.data.append(cy)
# print(a.data, area, cx, cy)
# # pub_size.publish(a)
# print ("Obj found in lower area\n")
# cv2.drawContours(frame_cpy, contours_lower, -1, (0,255,0), 3)
if abs(cx - default_x) > 20:
cv2.arrowedLine(frame_cpy, (default_x, 80),
(width - cx, 80), (255, 0, 100),
thickness=8)
else:
# a = Float64MultiArray()
# a.data = [0, 0.0,default_x,default_y]
a.data.append(0)
a.data.append(0.0)
a.data.append(default_x)
a.data.append(default_y)
# pub_size.publish(b)
# print ("Noise only at lower area\n")
# print(area, cx, cy)
else:
print("error")
# a = Float64MultiArray()
# a.data = [0, 0.0,default_x,default_y]
a.data.append(0)
a.data.append(0.0)
a.data.append(0.0)
a.data.append(0.0)
# print ("No target found at lower\n")
# # print(area)
# higher area publish
if len(contours_h) != 0:
c = max(contours_h, key=cv2.contourArea)
# cnt = c
# print(cnt)
area = cv2.contourArea(c)
M = cv2.moments(c)
if area != 0:
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
else:
cx = default_x
cy = default_y
if area > min_track_size:
a.data.append(1)
a.data.append(area)
a.data.append(cx)
a.data.append(cy)
# print(a.data, area, cx, cy)
# print(area, cx, cy)
# pub_size.publish(a)
# print ("Obj found in higher area\n")
else:
a.data.append(1)
a.data.append(0.0)
a.data.append(0.0)
a.data.append(0.0)
# print ("Noise only at higher area\n")
# print(area, cx, cy)
else:
print("error")
# a = Float64MultiArray()
# a.data = [1, 0.0,default_x,default_y]
a.data.append(1)
a.data.append(0.0)
a.data.append(0.0)
a.data.append(0.0)
# print ("No target found at higher\n")
# Obj area publish
if len(obj_contours) != 0:
c = max(obj_contours, key=cv2.contourArea)
# cnt = c
# print(cnt)
area = cv2.contourArea(c)
M = cv2.moments(c)
if area != 0:
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
else:
cx = default_x
cy = default_y
if area > min_track_size:
# a = Float64MultiArray()
# a.data = [area,cx, cy]
a.data.append(2)
a.data.append(area)
a.data.append(cx)
a.data.append(cy)
# print(area, cx, cy)
# pub_size.publish(a)
print("Obj found in obj area\n")
else:
# a = Float64MultiArray()
# a.data = [2, 0.0,default_x,default_y]
a.data.append(2)
a.data.append(0.0)
a.data.append(default_x)
a.data.append(default_y)
# pub_size.publish(b)
print("Noise only at obj area\n")
# print(area, cx, cy)
else:
print("error")
# a = Float64MultiArray()
# a.data = [0, 0.0,default_x,default_y]
a.data.append(0)
a.data.append(0.0)
a.data.append(0.0)
a.data.append(0.0)
print("No target found at obj\n")
# print(area)
print(a)
pub_size.publish(a)
# cv2.imshow('raw', frame_raw)
# cv2.imshow('res', res)
cv2.imshow('gray', gray_lower)
# cv2.imshow('gray_h', gray_h)
# cv2.imshow('canny', canny)
cv2.imshow('obj_gray', obj_gray)
# cv2.imshow("obj_res",obj_frame_hsv)
# cv2.imshow("original", frame_raw)
cv2.namedWindow('wb', cv2.WINDOW_NORMAL)
cv2.resizeWindow('wb', 1280, 960)
cv2.imshow("wb", frame_cpy)
cv2.waitKey(1)
# print(a)
except CvBridgeError as e:
print(e)
def tick(self):
"""Called every so often to update our state."""
if self.state == ForceFeedbackState.NORMAL:
# send desired position to IK directly
# Actually publish if we have some publishers
rospy.logerr('Im in NORMAL state')
if self.pubs is not None:
p0 = self.desired_pos
# Record the current sent pos as element 0 of self.prev_pos
# shuffling the other pos to the right. Then, truncate the
# list to be a maximum of 2 pos long.
self.prev_pos = [p0] + self.prev_pos
self.prev_pos = self.prev_pos[:2]
#Publish the desired position to /pos_for_IK
p0_list = p0.tolist()
msg = Float64MultiArray()
msg.data = deepcopy(p0_list)
self.pubs.publish(msg)
elif self.state == ForceFeedbackState.FORCEFB:
rospy.logerr('Im in FORCEFB state')
# Calculate correction vector
# Need to check whether desired_pos is nan. Can use one only because the function only allows scalar
if math.isnan(self.desired_pos[0]) == False:
self.stored_desired_pos = self.desired_pos
correction = self.stored_desired_pos - self.camera_pos
correction_mag = np.sqrt(np.sum(correction * correction))
#separate out two terms for testing and simplicity (and also because error in camera measurements in 2 dimensions)
correction_x_mag = np.sqrt(correction[0] * correction[0])
correction_z_mag = np.sqrt(correction[2] * correction[2])
rospy.logerr('correction_z_mag = ')
rospy.logerr(correction_z_mag)
#if the correction_vector bigger and smaller than some threshold, then do something
#if math.isnan(correction_mag)==True or correction_mag <= epsilon or correction_mag >= 0.05:
#need correction_z_mag in case we are using force feedback on the wrong pose
if math.isnan(correction_x_mag) == False and math.isnan(
correction_z_mag) == False and correction_z_mag <= 0.15:
if correction_x_mag >= 0.04 or correction_z_mag >= 0.03:
rospy.logerr('FORCE FEEDBACK with correction mag: ' +
str(correction_mag))
# Calculate new commanded pose
p0 = self.stored_desired_pos
p0_new = p0 + 1.0 * correction #the constant can be changed depending on needs
self.prev_pos = [p0_new] + self.prev_pos
self.prev_pos = self.prev_pos[:2]
#Publish the new p0 to /pos_for_IK
p0_list = p0_new.tolist()
msg = Float64MultiArray()
msg.data = deepcopy(p0_list)
rospy.logerr('Publishing msg: ' + str(msg))
self.pubs.publish(msg)
else:
rospy.logerr('TRIED FORCE FB BUT NOT REQUIRED')
p0 = self.desired_pos
# Record the current sent pos as element 0 of self.prev_pos
# shuffling the other pos to the right. Then, truncate the
# list to be a maximum of 2 pos long.
self.prev_pos = [p0] + self.prev_pos
self.prev_pos = self.prev_pos[:2]
#Publish the desired position to /pos_for_IK
p0_list = p0.tolist()
msg = Float64MultiArray()
msg.data = deepcopy(p0_list)
self.pubs.publish(msg)
else:
rospy.logerr(
'MATH IS NAN or wrong pose, equivalent to NORMAL state')
p0 = self.desired_pos
# Record the current sent pos as element 0 of self.prev_pos
# shuffling the other pos to the right. Then, truncate the
# list to be a maximum of 2 pos long.
self.prev_pos = [p0] + self.prev_pos
self.prev_pos = self.prev_pos[:2]
#Publish the desired position to /pos_for_IK
p0_list = p0.tolist()
msg = Float64MultiArray()
msg.data = deepcopy(p0_list)
self.pubs.publish(msg)
self.desired_pos = np.ones(3) * np.nan
self.state = ForceFeedbackState.NORMAL
#Wait for 3 sec for arm to get to init pos
time_start = rospy.get_time()
goal_time = time_start + 3
while(rospy.get_time() < goal_time):
rate.sleep()
#goalLoc = np.array([0.135, 0.28185, 0])
goalLoc = np.array([0.3426, 0.08945, 0])
#goalLoc = np.array([0.235, -0.01815, 0])
#goalLoc = np.array([0.18945, 0.18945, 0])
finalAngles = move_arm(goalLoc, jointpub, initialAngles)
'''goalLoc = np.array([0.09, -0.16, 0])
finalAngles = move_arm(goalLoc, jointpub, finalAngles)'''
#Execute for 5 seconds to let the arm get to destination pos
time_start = rospy.get_time()
goal_time = time_start + 5
while(rospy.get_time() < goal_time):
rate.sleep()
joint_pos = Float64MultiArray()
res, pl = clean_joint_states(finalAngles)
joint_pos.data = np.concatenate((np.array([0]), np.concatenate((res, np.array([0.34])))))
jointpub.publish(joint_pos)
#Allow for 3 seconds for the grapple to grab the lego, may need smoothing
time_start = rospy.get_time()
goal_time = time_start + 3
while(rospy.get_time() < goal_time):
rate.sleep()
def move_arm(goalPos, jointpub, initAngles):
joint_pos = Float64MultiArray()
# Joint Position vector should contain 6 elements:
# [0, shoulder1, shoulder2, elbow, wrist, gripper]
#3rd -1.22 for ground
#0.34 for grip
initialAngles = initAngles
arm = RobotArm()
#forward_kinematics = compute_forward_kinematics(joint_states)
# Forward kinematics computation
target = goalPos
T = arm.forwardKinematics(initialAngles[0], initialAngles[1], initialAngles[2],
initialAngles[3])
# x,y coords for all the joints
joint2pos, joint3pos, joint4pos, endEffectorPos = arm.findJointPos()
newAngles = initialAngles
endEffectorPosInit = endEffectorPos
#Number of IK iterations
numIT = 20
for i in range(numIT):
# obtain the Jacobian
J = computeGeomJacobian(joint2pos, joint3pos, joint4pos, endEffectorPos)
print("Jointpos")
print(joint2pos)
print(joint3pos)
print(joint4pos)
print(endEffectorPos)
# compute the dy steps
newgoal = endEffectorPosInit + (i*(target - endEffectorPosInit))/numIT
deltaStep = newgoal - endEffectorPos
# define the dy
subtarget = np.array([deltaStep[0], deltaStep[1], deltaStep[2]])
# compute dq from dy and pseudo-Jacobian
angleChange = np.dot(np.linalg.pinv(J), subtarget)
print(angleChange[0])
angleChange = np.array([angleChange[0], angleChange[1], angleChange[2],
angleChange[3]])
# update the q
tempAngles, areValidAngles = clean_joint_states(newAngles + angleChange)
tempAngles = np.array(tempAngles)
if areValidAngles == True:
prevAngles = newAngles
newAngles = tempAngles
# Do forward kinematics for a set angle on each joint
T = arm.forwardKinematics(newAngles[0],newAngles[1],newAngles[2],newAngles[3])
# Find the x,y coordinates of joints 2, 3 and end effector so they can be plotted
joint2pos, joint3pos, joint4pos, endEffectorPos = arm.findJointPos()
interpolate_angles(prevAngles, newAngles, 200, 200)
else:
print("Hitting rotational limits!")
continue
return newAngles
def callback2(self, data):
# Recieve the image
try:
self.image2 = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
#Blob detection starts here-------------------------------------------------------
#Same to 2_1_joint_estimation.py
def detect_red(self, image1, image2):
image_gau_blur1 = cv2.GaussianBlur(image1, (1, 1), 0)
hsv1 = cv2.cvtColor(image_gau_blur1, cv2.COLOR_BGR2HSV)
lower_red1 = np.array([0, 200, 0])
higher_red1 = np.array([0, 255, 255])
red_range1 = cv2.inRange(hsv1, lower_red1, higher_red1)
res_red1 = cv2.bitwise_and(image_gau_blur1,
image_gau_blur1,
mask=red_range1)
red_s_gray1 = cv2.cvtColor(res_red1, cv2.COLOR_BGR2GRAY)
canny_edge1 = cv2.Canny(red_s_gray1, 30, 70)
contours1, hierarchy1 = cv2.findContours(canny_edge1,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
(x1, y1), radius1 = cv2.minEnclosingCircle(contours1[0])
cy, cz1 = (int(x1), int(y1))
radius1 = int(radius1)
image_gau_blur2 = cv2.GaussianBlur(image2, (1, 1), 0)
hsv2 = cv2.cvtColor(image_gau_blur2, cv2.COLOR_BGR2HSV)
lower_red2 = np.array([0, 200, 0])
higher_red2 = np.array([0, 255, 255])
red_range2 = cv2.inRange(hsv2, lower_red2, higher_red2)
res_red2 = cv2.bitwise_and(image_gau_blur2,
image_gau_blur2,
mask=red_range2)
red_s_gray2 = cv2.cvtColor(res_red2, cv2.COLOR_BGR2GRAY)
canny_edge2 = cv2.Canny(red_s_gray2, 30, 70)
contours2, hierarchy2 = cv2.findContours(canny_edge2,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
(x2, y2), radius2 = cv2.minEnclosingCircle(contours2[0])
cx, cz2 = (int(x2), int(y2))
radius2 = int(radius2)
return np.array([cx, cy, cz1, cz2])
def detect_blue(self, image1, image2):
image_gau_blur1 = cv2.GaussianBlur(image1, (1, 1), 0)
hsv1 = cv2.cvtColor(image_gau_blur1, cv2.COLOR_BGR2HSV)
lower_red1 = np.array([70, 0, 0])
higher_red1 = np.array([255, 255, 255])
red_range1 = cv2.inRange(hsv1, lower_red1, higher_red1)
res_red1 = cv2.bitwise_and(image_gau_blur1,
image_gau_blur1,
mask=red_range1)
red_s_gray1 = cv2.cvtColor(res_red1, cv2.COLOR_BGR2GRAY)
canny_edge1 = cv2.Canny(red_s_gray1, 30, 70)
contours1, hierarchy1 = cv2.findContours(canny_edge1,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
(x1, y1), radius1 = cv2.minEnclosingCircle(contours1[0])
cy, cz1 = (int(x1), int(y1))
radius1 = int(radius1)
image_gau_blur2 = cv2.GaussianBlur(image2, (1, 1), 0)
hsv2 = cv2.cvtColor(image_gau_blur2, cv2.COLOR_BGR2HSV)
lower_red2 = np.array([70, 0, 0])
higher_red2 = np.array([255, 255, 255])
red_range2 = cv2.inRange(hsv2, lower_red2, higher_red2)
res_red2 = cv2.bitwise_and(image_gau_blur2,
image_gau_blur2,
mask=red_range2)
red_s_gray2 = cv2.cvtColor(res_red2, cv2.COLOR_BGR2GRAY)
canny_edge2 = cv2.Canny(red_s_gray2, 30, 70)
contours2, hierarchy2 = cv2.findContours(canny_edge2,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
(x2, y2), radius2 = cv2.minEnclosingCircle(contours2[0])
cx, cz2 = (int(x2), int(y2))
radius2 = int(radius2)
return np.array([cx, cy, cz1, cz2])
def detect_green(self, image1, image2):
image_gau_blur1 = cv2.GaussianBlur(image1, (1, 1), 0)
hsv1 = cv2.cvtColor(image_gau_blur1, cv2.COLOR_BGR2HSV)
lower_red1 = np.array([55, 0, 0])
higher_red1 = np.array([100, 255, 255])
red_range1 = cv2.inRange(hsv1, lower_red1, higher_red1)
res_red1 = cv2.bitwise_and(image_gau_blur1,
image_gau_blur1,
mask=red_range1)
red_s_gray1 = cv2.cvtColor(res_red1, cv2.COLOR_BGR2GRAY)
canny_edge1 = cv2.Canny(red_s_gray1, 30, 70)
contours1, hierarchy1 = cv2.findContours(canny_edge1,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
(x1, y1), radius1 = cv2.minEnclosingCircle(contours1[0])
cy, cz1 = (int(x1), int(y1))
radius1 = int(radius1)
image_gau_blur2 = cv2.GaussianBlur(image2, (1, 1), 0)
hsv2 = cv2.cvtColor(image_gau_blur2, cv2.COLOR_BGR2HSV)
lower_red2 = np.array([55, 0, 0])
higher_red2 = np.array([100, 255, 255])
red_range2 = cv2.inRange(hsv2, lower_red2, higher_red2)
res_red2 = cv2.bitwise_and(image_gau_blur2,
image_gau_blur2,
mask=red_range2)
red_s_gray2 = cv2.cvtColor(res_red2, cv2.COLOR_BGR2GRAY)
canny_edge2 = cv2.Canny(red_s_gray2, 30, 70)
contours2, hierarchy2 = cv2.findContours(canny_edge2,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
(x2, y2), radius2 = cv2.minEnclosingCircle(contours2[0])
cx, cz2 = (int(x2), int(y2))
radius2 = int(radius2)
return np.array([cx, cy, cz1, cz2])
def detect_yellow(self, image1, image2):
image_gau_blur1 = cv2.GaussianBlur(image1, (1, 1), 0)
hsv1 = cv2.cvtColor(image_gau_blur1, cv2.COLOR_BGR2HSV)
lower_red1 = np.array([16, 244, 0])
higher_red1 = np.array([51, 255, 255])
red_range1 = cv2.inRange(hsv1, lower_red1, higher_red1)
res_red1 = cv2.bitwise_and(image_gau_blur1,
image_gau_blur1,
mask=red_range1)
red_s_gray1 = cv2.cvtColor(res_red1, cv2.COLOR_BGR2GRAY)
canny_edge1 = cv2.Canny(red_s_gray1, 30, 70)
contours1, hierarchy1 = cv2.findContours(canny_edge1,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
(x1, y1), radius1 = cv2.minEnclosingCircle(contours1[0])
cy, cz1 = (int(x1), int(y1))
radius1 = int(radius1)
image_gau_blur2 = cv2.GaussianBlur(image2, (1, 1), 0)
hsv2 = cv2.cvtColor(image_gau_blur2, cv2.COLOR_BGR2HSV)
lower_red2 = np.array([16, 244, 0])
higher_red2 = np.array([51, 255, 255])
red_range2 = cv2.inRange(hsv2, lower_red2, higher_red2)
res_red2 = cv2.bitwise_and(image_gau_blur2,
image_gau_blur2,
mask=red_range2)
red_s_gray2 = cv2.cvtColor(res_red2, cv2.COLOR_BGR2GRAY)
canny_edge2 = cv2.Canny(red_s_gray2, 30, 70)
contours2, hierarchy2 = cv2.findContours(canny_edge2,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
(x2, y2), radius2 = cv2.minEnclosingCircle(contours2[0])
cx, cz2 = (int(x2), int(y2))
radius2 = int(radius2)
return np.array([cx, cy, cz1, cz2])
def detect_blue_contours(image1):
image_gau_blur1 = cv2.GaussianBlur(image1, (1, 1), 0)
hsv1 = cv2.cvtColor(image_gau_blur1, cv2.COLOR_BGR2HSV)
lower_red1 = np.array([70, 0, 0])
higher_red1 = np.array([255, 255, 255])
red_range1 = cv2.inRange(hsv1, lower_red1, higher_red1)
res_red1 = cv2.bitwise_and(image_gau_blur1,
image_gau_blur1,
mask=red_range1)
red_s_gray1 = cv2.cvtColor(res_red1, cv2.COLOR_BGR2GRAY)
canny_edge1 = cv2.Canny(red_s_gray1, 30, 70)
contours1, hierarchy1 = cv2.findContours(canny_edge1,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
return np.array([contours1])
def detect_yellow_contours(image1):
image_gau_blur1 = cv2.GaussianBlur(image1, (1, 1), 0)
hsv1 = cv2.cvtColor(image_gau_blur1, cv2.COLOR_BGR2HSV)
lower_red1 = np.array([16, 244, 0])
higher_red1 = np.array([51, 255, 255])
red_range1 = cv2.inRange(hsv1, lower_red1, higher_red1)
res_red1 = cv2.bitwise_and(image_gau_blur1,
image_gau_blur1,
mask=red_range1)
red_s_gray1 = cv2.cvtColor(res_red1, cv2.COLOR_BGR2GRAY)
canny_edge1 = cv2.Canny(red_s_gray1, 30, 70)
contours1, hierarchy1 = cv2.findContours(canny_edge1,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
(x1, y1), radius1 = cv2.minEnclosingCircle(contours1[0])
cy, cz1 = (int(x1), int(y1))
return np.array([contours1])
def get_y1_y2(yellow_contours, blue_contours):
y1 = np.min(yellow_contours, axis=0)
y1 = y1[0][1]
y1 = y1[:, 1]
y2 = np.max(blue_contours, axis=0)
y2 = y2[0][1]
y2 = y2[:, 1]
return y1, y2
def pixelTometer(self, image1, image2):
yellow_contours = detect_yellow_contours(image2)
blue_contours = detect_blue_contours(image2)
y2 = detect_blue(self, image1, image2)
y2 = y2[3]
y1, y2 = get_y1_y2(yellow_contours, blue_contours)
p2m = 2.5 / (y1 - y2)
#65 is the best number
return p2m
#----------------------------------------------------------------------------------------------
#Angle Detection starts here
#This part is same as 2_1_joint_estimation.py
def detect_angles_blob(self, image1, image2):
try:
p = pixelTometer(self, image1, image2)
self.p2m = p
except Exception as e:
p = self.p2m
try:
green = detect_green(self, image1, image2)
self.green = green
except Exception as e:
green = self.green
try:
red = detect_red(self, image1, image2)
self.red = red
except Exception as e:
red = self.red
p = pixelTometer(self, image1, image2)
yellow = p * detect_yellow(self, image1, image2)
blue = p * detect_blue(self, image1, image2)
ja1 = 0.0
ja2 = np.pi / 2 - np.arctan2((blue[2] - green[2]),
(blue[1] - green[1]))
ja3 = np.arctan2((blue[3] - green[3]),
(blue[0] - green[0])) - np.pi / 2
ja4 = np.arctan2(
(green[2] - red[2]), -(green[1] - red[1])) - np.pi / 2 - ja2
return np.array([ja1, ja2, ja3, ja4])
def angle_trajectory(self):
curr_time = np.array([rospy.get_time() - self.time_trajectory])
ja1 = 0.1
ja2 = float((np.pi / 2) * np.sin((np.pi / 15) * curr_time))
ja3 = float((np.pi / 2) * np.sin((np.pi / 18) * curr_time))
ja4 = float((np.pi / 2) * np.sin((np.pi / 20) * curr_time))
return np.array([ja1, ja2, ja3, ja4])
def actual_target_position(self):
curr_time = np.array([rospy.get_time() - self.time_trajectory])
x_d = float((2.5 * np.cos(curr_time * np.pi / 15)) + 0.5)
y_d = float(2.5 * np.sin(curr_time * np.pi / 15))
z_d = float((1 * np.sin(curr_time * np.pi / 15)) + 7.0)
return np.array([x_d, y_d, z_d])
#FK starts here--------------------------------------------------------------------------------
#This part is same as 3_1_FK.py
def end_effector_position(self, image1, image2):
try:
p = pixelTometer(self, image1, image2)
self.p2m = p
except Exception as e:
p = self.p2m
yellow_posn = detect_yellow(self, image1, image2)
red_posn = detect_red(self, image1, image2)
yellow_posn[3] = 800 - yellow_posn[3]
red_posn[3] = 800 - red_posn[3]
cx, cy, cz1, cz2 = p * (red_posn - yellow_posn)
ee_posn = np.array([cx, cy, cz2])
ee_posn = np.round(ee_posn, 1)
return ee_posn
#Calculate the jacobian
def calculate_jacobian(self, image1, image2):
ja1, ja2, ja3, ja4 = detect_angles_blob(self, image1, image2)
jacobian = np.array(
[[
3 * np.cos(ja1) * np.sin(ja2) * np.cos(ja3) * np.cos(ja4) +
3.5 * np.cos(ja1) * np.sin(ja2) * np.cos(ja3) -
3 * np.sin(ja1) * np.cos(ja4) * np.sin(ja3) -
3.5 * np.sin(ja1) * np.sin(ja3) +
3 * np.cos(ja1) * np.cos(ja2) * np.sin(ja4),
3 * np.sin(ja1) * np.cos(ja2) * np.cos(ja3) * np.cos(ja4) +
3.5 * np.sin(ja1) * np.cos(ja2) * np.cos(ja3) -
3 * np.sin(ja1) * np.sin(ja2) * np.sin(ja4),
-3 * np.sin(ja1) * np.sin(ja2) * np.sin(ja3) * np.cos(ja4)
- 3.5 * np.sin(ja1) * np.sin(ja2) * np.sin(ja3) +
3 * np.cos(ja1) * np.cos(ja4) * np.cos(ja3) +
3.5 * np.cos(ja1) * np.cos(ja3),
-3 * np.sin(ja1) * np.sin(ja2) * np.cos(ja3) * np.sin(ja4)
- 3 * np.cos(ja1) * np.sin(ja4) * np.sin(ja3) +
3 * np.sin(ja1) * np.cos(ja2) * np.cos(ja4)
],
[
3 * np.sin(ja1) * np.sin(ja2) * np.cos(ja3) * np.cos(ja4)
+ 3.5 * np.sin(ja1) * np.sin(ja2) * np.cos(ja3) +
3 * np.cos(ja1) * np.cos(ja4) * np.sin(ja3) +
3.5 * np.cos(ja1) * np.sin(ja3) +
3 * np.sin(ja1) * np.cos(ja2) * np.sin(ja4),
-3 * np.cos(ja1) * np.cos(ja2) * np.cos(ja3) * np.cos(ja4)
- 3.5 * np.cos(ja1) * np.cos(ja2) * np.cos(ja3) +
3 * np.cos(ja1) * np.sin(ja2) * np.sin(ja4),
+3 * np.cos(ja1) * np.sin(ja2) * np.sin(ja3) * np.cos(ja4)
+ 3.5 * np.cos(ja1) * np.sin(ja2) * np.sin(ja3) +
3 * np.sin(ja1) * np.cos(ja4) * np.cos(ja3) +
3.5 * np.sin(ja1) * np.cos(ja3),
+3 * np.cos(ja1) * np.sin(ja2) * np.cos(ja3) * np.sin(ja4)
- 3 * np.sin(ja1) * np.sin(ja4) * np.sin(ja3) -
3 * np.cos(ja1) * np.cos(ja2) * np.cos(ja4)
],
[
0, -3 * np.cos(ja3) * np.cos(ja4) * np.sin(ja2) -
3.5 * np.cos(ja3) * np.sin(ja2) -
3 * np.sin(ja4) * np.cos(ja2),
-3 * np.sin(ja3) * np.cos(ja4) * np.cos(ja2) -
3.5 * np.sin(ja3) * np.cos(ja2),
-3 * np.cos(ja3) * np.sin(ja4) * np.cos(ja2) -
3 * np.cos(ja4) * np.sin(ja2)
]])
return jacobian
#Target Detection starts here---------------------------------------------------------------------
#Same as 2_2_target_detection
def initialize_detect_shape_var(template):
startX = []
startY = []
endX = []
endY = []
w, h = template.shape[::-1]
return startX, startY, endX, endY, w, h
def get_resized_ratio(mask, scale):
found = None
resized = imutils.resize(mask, width=int(mask.shape[1] * scale))
r = mask.shape[1] / float(resized.shape[1])
return resized, r, found
def get_maxVal_maxLoc(resized, template):
res = cv2.matchTemplate(resized, template, cv2.TM_CCOEFF_NORMED)
(_, maxVal, _, maxLoc) = cv2.minMaxLoc(res)
return maxVal, maxLoc
def append_X_Y(startX, startY, endX, endY, maxLoc, r, w, h):
startX.append(int(maxLoc[0] * r))
startY.append(int(maxLoc[1] * r))
endX.append(int((maxLoc[0] + w) * r))
endY.append(int((maxLoc[1] + h) * r))
return startX, startY, endX, endY
def detect_shape(self, mask, template):
startX, startY, endX, endY, w, h = initialize_detect_shape_var(
template)
for scale in np.linspace(0.79, 1.0, 5)[::-1]:
resized, r, found = get_resized_ratio(mask, scale)
if resized.shape[0] < h or resized.shape[1] < w:
break
maxVal, maxLoc = get_maxVal_maxLoc(resized, template)
if found is None or maxVal > found[0]:
found = (maxVal, maxLoc, r)
startX, startY, endX, endY = append_X_Y(
startX, startY, endX, endY, maxLoc, r, w, h)
centerX = (statistics.mean(endX) + statistics.mean(startX)) / 2
centerY = (statistics.mean(endY) + statistics.mean(startY)) / 2
return centerX, centerY
def apply_mask_target(image):
hsv_convert = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv_convert, (10, 202, 0), (27, 255, 255))
kernel = np.ones((2, 2), np.uint8)
mask = cv2.dilate(mask, kernel, iterations=1)
return mask
def get_target_location(self, centerX, centerY, centerZ):
base_location = detect_yellow(self, self.image1, self.image2)
base_location[2] = 800 - base_location[2]
centerZ = 800 - centerZ
target_location = (centerX - base_location[0],
centerY - base_location[1],
centerZ - base_location[2])
target_location = np.asarray(target_location)
return target_location
def flying_object_location(self, image1, image2, template, threshold):
mask1 = apply_mask_target(image1)
mask2 = apply_mask_target(image2)
centerY, centerZ1 = detect_shape(self, mask1, template)
centerX, centerZ2 = detect_shape(self, mask2, template)
try:
p = pixelTometer(self, image1, image2)
self.p2m = p
except Exception as e:
p = self.p2m
image1 = cv2.circle(image1, (int(centerY), int(centerZ1)),
radius=2,
color=(255, 255, 255),
thickness=-1)
image2 = cv2.circle(image2, (int(centerX), int(centerZ2)),
radius=2,
color=(255, 255, 255),
thickness=-1)
target_location = get_target_location(self, centerX, centerY,
centerZ1)
target_location_meters = target_location * p
return target_location_meters
#Control Part starts here------------------------------------------------------------------------------------------
#Estimate control inputs for open-loop control
def control_closed(self, image1, image2):
# P gain
K_p = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
# D gain
K_d = np.array([[0.1, 0, 0], [0, 0.1, 0], [0, 0, 0.1]])
# estimate time step
cur_time = np.array([rospy.get_time()])
dt = cur_time - self.time_previous_step
self.time_previous_step = cur_time
# robot end-effector position
pos = end_effector_position(self, image1, image2)
# desired trajectory
template = cv2.imread("cropped.png", 0)
pos_d = flying_object_location(self, image1, image2, template, 0.8)
# estimate derivative of error
self.error_d = ((pos_d - pos) - self.error) / dt
# estimate error
self.error = pos_d - pos
q = detect_angles_blob(self, image1,
image2) # estimate initial value of joints'
J_inv = np.linalg.pinv(calculate_jacobian(
self, image1,
image2)) # calculating the psudeo inverse of Jacobian
dq_d = np.dot(J_inv,
(np.dot(K_d, self.error_d.transpose()) +
np.dot(K_p, self.error.transpose())
)) # control input (angular velocity of joints)
q_d = q + (dt * dq_d) # control input (angular position of joints)
return q_d
cv2.waitKey(1)
#Publishing data starts here-----------------------------
q_d = control_closed(self, self.image1, self.image2)
x_a = actual_target_position(self)
self.actual_target = Float64MultiArray()
self.actual_target.data = x_a
self.joints = Float64MultiArray()
self.joints.data = detect_angles_blob(self, self.image1, self.image2)
ja1, ja2, ja3, ja4 = angle_trajectory(self)
self.joint1 = Float64()
self.joint1.data = q_d[0]
self.joint2 = Float64()
self.joint2.data = q_d[1]
self.joint3 = Float64()
self.joint3.data = q_d[2]
self.joint4 = Float64()
self.joint4.data = q_d[3]
vision_end_effector = end_effector_position(self, self.image1,
self.image2)
self.vision_EF = Float64MultiArray()
self.vision_EF.data = vision_end_effector
# # Publishing the desired trajectory on a topic named trajectory(for lab 3)
template = cv2.imread("cropped.png", 0)
x_d = flying_object_location(self, self.image1, self.image2, template,
0.8)
self.vision_target = Float64MultiArray()
self.vision_target.data = x_d
try:
self.image_pub1.publish(
self.bridge.cv2_to_imgmsg(self.image1, "bgr8"))
self.image_pub2.publish(
self.bridge.cv2_to_imgmsg(self.image2, "bgr8"))
self.vision_end_effector_pub.publish((self.vision_EF))
self.vision_target_trajectory_pub.publish((self.vision_target))
self.actual_target_trajectory_pub.publish((self.actual_target))
self.joints_pub.publish(self.joints)
self.robot_joint1_pub.publish(self.joint1)
self.robot_joint2_pub.publish(self.joint2)
self.robot_joint3_pub.publish(self.joint3)
self.robot_joint4_pub.publish(self.joint4)
except CvBridgeError as e:
print(e)
def control_callback(event):
# Actual control is done here. The 'event' is the rospy.Timer() duration period, can be used
# for trubleshooting. To test how often is executed use: $ rostopic hz /mallard/thruster_commands.
global x0,y0,q0,x_goal,y_goal,q_goal,ed
global time_elapsed,t_goal,t_goal_psi
global t_ramp,psivel
twist = Twist()
# Get forces in global frame using PD controller
if(goals_received == True and joy_button_L1 == 0 and joy_button_R1 == 0):
# new code - velocity ramp
# get current time
tn = rospy.Time.now()
time_now = tn.secs + (tn.nsecs * 0.000000001)
# time elapsed form the start of tracking the goal (equal to t_now in goal selector)
time_elapsed = time_now - time_begin
# get max velocities
xvelmax = abs(kguseful.safe_div((x_goal-x0),t_goal))
yvelmax = abs(kguseful.safe_div((y_goal-y0),t_goal))
# get desired position,velocity and acceleration from velocity ramp
x_des, x_vel_des, x_acc_des = kglocal.velramp(time_elapsed, xvelmax, x0, x_goal, t_ramp,name="x")
y_des, y_vel_des, y_acc_des = kglocal.velramp(time_elapsed, yvelmax, y0, y_goal, t_ramp,name="y")
# get desired angle:
qdes = kglocal.despsi_fun(q_goal, t_goal_psi, q0, time_elapsed)
psi_des = tft.euler_from_quaternion(qdes)[2] # Its a list: (roll,pitch,yaw)
# psi_des = psides[2]
psi_vel_des = kglocal.desvelpsi_fun(ed, t_goal_psi, time_elapsed,psivel)
# end new code
# PD control
# x_global_ctrl = control.proportional(x, x_goal, x_vel, x_vel_goal, param['kp'], param['kd'], param['lim'])
# y_global_ctrl = control.proportional(y, y_goal, y_vel, y_vel_goal, param['kp'], param['kd'], param['lim'])
psi_global_ctrl = control.proportional_angle(psi, psi_des,psi_vel,psi_vel_des, param['kp_psi'], param['kd_psi'], param['lim_psi'])
# PD body control
# x_PD_body = math.cos(psi)*x_global_ctrl + math.sin(psi)*y_global_ctrl
# y_PD_body =-math.sin(psi)*x_global_ctrl + math.cos(psi)*y_global_ctrl
# ----- Model control -----
ax = control.acc_ctrl(x, x_des, x_vel, x_vel_des,x_acc_des,param_model_x['kp'], param_model_x['kd'], param_model_x['lim'])
ay = control.acc_ctrl(y, y_des, y_vel, y_vel_des,y_acc_des,param_model_y['kp'], param_model_y['kd'], param_model_y['lim'])
# Model body control:
# aqx = Rt * [ax,ay]t
# vqx = Rt * [x_vel,y_vel]t
aqx = math.cos(psi)*ax + math.sin(psi)*ay
aqy = -math.sin(psi)*ax + math.cos(psi)*ay
vqx = math.cos(psi)*x_vel + math.sin(psi)*y_vel
vqy = -math.sin(psi)*x_vel + math.cos(psi)*y_vel
# print("X-velocity: " + str(round(vqx,4)))
x_body_model_ctrl = Mx*aqx + m22*vqy*psi_vel +R1_x*vqx + R2_x*(vqx*abs(vqx))
# x_body_model_ctrl = Mx*aqx + R2_x*(vqx*abs(vqx))
y_body_model_ctrl = My*aqy + R1_y*vqy + R2_y*(vqy*abs(vqy))
# y_body_model_ctrl = My*aqy + R2_y*(vqy*abs(vqy))
# twist.angular.y = data.buttons[4]
# vector forces scaled in body frame
twist.linear.x = x_body_model_ctrl
# twist.linear.x = x_PD_body
twist.linear.y = y_body_model_ctrl
# twist.linear.y = y_PD_body
twist.angular.z = -psi_global_ctrl
# Test uniformity of the timer
now = rospy.Time.now()
# twist.angular.x = now.secs
# twist.angular.y = now.nsecs
# Publish forces to simulation (joint_state_publisher message)
pub_velocity.publish(twist)
# send [time,position,velocity,goal_position,goal_velocity,control input]
array_data = [now.secs,now.nsecs,\
x,y,psi,x_vel,y_vel,psi_vel,\
x_des,y_des,psi_des,x_vel_des,y_vel_des,psi_vel_des,\
x_body_model_ctrl,y_body_model_ctrl,psi_global_ctrl]
data_to_send = Float64MultiArray(data = array_data)
pub_data.publish(data_to_send)
# goals_received, xdes,ydes,psides[2],\
# xveldes,yveldes,psiveldes,ax,ay]
elif(joy_button_L1 == 1 or joy_button_R1 == 1):
twist.linear.x = joy_x
twist.linear.y = joy_y
twist.angular.z = joy_z
pub_velocity.publish(twist)
else:
# ----- idle if no goals -----
pub_velocity.publish(Twist())
def __init__(self, mode="hs"):
self.init = False
self.name = "lpzros"
self.mode = LPZRosEH.modes[mode]
self.cnt = 0
############################################################
# ros stuff
rospy.init_node(self.name)
# pub=rospy.Publisher("/motors", Float64MultiArray, queue_size=1)
self.pub_motors = rospy.Publisher("/motors", Float64MultiArray)
self.pub_res_r = rospy.Publisher("/reservoir/r", Float64MultiArray)
self.pub_res_w = rospy.Publisher("/reservoir/w", Float64MultiArray)
self.pub_res_perf = rospy.Publisher("/reservoir/perf", Float64MultiArray)
self.pub_res_perf_lp = rospy.Publisher("/reservoir/perf_lp", Float64MultiArray)
self.pub_res_mdltr = rospy.Publisher("/reservoir/mdltr", Float64MultiArray)
self.sub_sensor = rospy.Subscriber("/sensors", Float64MultiArray, self.cb_sensors)
# pub=rospy.Publisher("/chatter", Float64MultiArray)
self.msg = Float64MultiArray()
self.msg_res_r = Float64MultiArray()
self.msg_res_w = Float64MultiArray()
self.msg_res_perf = Float64MultiArray()
self.msg_res_perf_lp = Float64MultiArray()
self.msg_res_mdltr = Float64MultiArray()
# controller
self.N = 200
self.p = 0.1
# self.g = 1.5
# self.g = 1.2
self.g = 0.999
# self.g = 0.001
# self.g = 0.
self.alpha = 1.0
self.wf_amp = 0.0
# self.wf_amp = 0.005
# self.wi_amp = 0.01
# self.wi_amp = 0.1
# self.wi_amp = 1.0 # barrel
self.wi_amp = 5.0
self.idim = 2
self.odim = 2
# simulation time / experiment duration
# self.nsecs = 500 #1440*10
# self.nsecs = 250
# self.nsecs = 100
self.nsecs = 50 #1440*10
self.dt = 0.005
self.dt = 0.01
self.learn_every = 1
self.test_rate = 0.95
self.washout_rate = 0.05
self.simtime = np.arange(0, self.nsecs, self.dt)
self.simtime_len = len(self.simtime)
self.simtime2 = np.arange(1*self.nsecs, 2*self.nsecs, self.dt)
# self.x0 = 0.5*np.random.normal(size=(self.N,1))
self.x0 = 0.5 * np.random.normal(size=(self.N, 1))
self.z0 = 0.5 * np.random.normal(size=(1, self.odim))
# EH stuff
self.z_t = np.zeros((2,self.simtime_len))
self.zn_t = np.zeros((2,self.simtime_len)) # noisy readout
# self.zp_t = np.zeros((2,self.simtime_len))
self.wo_len_t = np.zeros((2,self.simtime_len))
self.dw_len_t = np.zeros((2,self.simtime_len))
self.perf = np.zeros((1,self.odim))
self.perf_t = np.zeros((2,self.simtime_len))
self.mdltr = np.zeros((1,2))
self.mdltr_t = np.zeros((2,self.simtime_len))
self.r_t = np.zeros(shape=(self.N, self.simtime_len))
self.zn_lp = np.zeros((1,2))
self.perf_lp = np.zeros((1,2))
self.perf_lp_t = np.zeros((2,self.simtime_len))
# self.coeff_a = 0.2
# self.coeff_a = 0.1
self.coeff_a = 0.05
# self.coeff_a = 0.03
# self.coeff_a = 0.001
# self.coeff_a = 0.0001
# self.eta_init = 0.0001
# self.eta_init = 0.001 # limit energy in perf
self.eta_init = 0.0025 # limit energy in perf
# self.eta_init = 0.01 # limit energy in perf
self.T = 200000.
# self.T = 2000.
# exploration noise
# self.theta = 1.0
# self.theta = 1/4.
self.theta = 0.1
# self.state noise
self.theta_state = 0.1
# self.theta_state = 1e-3
# leaky integration time constant
# self.tau
# self.tau = 0.001
# self.tau = 0.005
self.tau = 0.2
# self.tau = 0.9
# self.tau = 0.99
# self.tau = 1.
############################################################
# alternative controller network / reservoir from lib
# FIXME: use config file
# FIXME: use input coupling matrix for non-homogenous input scaling
self.res = Reservoir2(N = self.N, p = self.p, g = self.g, alpha = 1.0, tau = self.tau,
input_num = self.idim, output_num = self.odim, input_scale = self.wi_amp,
feedback_scale = self.wf_amp, bias_scale = 0., eta_init = self.eta_init,
sparse=True)
self.res.theta = self.theta
self.res.theta_state = self.theta_state
self.res.coeff_a = self.coeff_a
self.res.x = self.x0
self.res.r = np.tanh(self.res.x)
self.res.z = self.z0
self.res.zn = np.atleast_2d(self.z0).T
# print res.x, res.r, res.z, res.zn
print ' N: ', self.N
print ' g: ', self.g
print ' p: ', self.p
print ' nsecs: ', self.nsecs
print ' learn_every: ', self.learn_every
print ' theta: ', self.theta
# set point for goal based learning
self.sp = 0.4
# explicit memory
# self.piwin = 100
# self.piwin = 200
# self.piwin = 500
# self.piwin = 1000
self.piwin = 2000
self.x_t = np.zeros((self.idim, self.piwin))
self.z_t = np.zeros((self.odim, self.piwin))
# self.wgt_lim = 0.5 # barrel
self.wgt_lim = 0.1
self.wgt_lim_inv = 1/self.wgt_lim
self.init = True
print "init done"
#!/usr/bin/env python # August Soderberg # [email protected] # Autonomous Robotics # PA - Line Follower # 10/23/2020 import rospy from nav_msgs.msg import Odometry from std_msgs.msg import Float64MultiArray from constants import * def odom_cb(msg): global x, y x, y = msg.pose.pose.position.x, msg.pose.pose.position.y rospy.init_node('gps_faker') sub = rospy.Subscriber('odom', Odometry, odom_cb) pub = rospy.Publisher('gps_data', Float64MultiArray, queue_size=1) rate = rospy.Rate(10) array = Float64MultiArray() y = 0 x = 0 while not rospy.is_shutdown(): array.data = [y, x, 4] pub.publish(array) rate.sleep()
def conversion():
#Iniciamos el nodo y declaramos las suscripciones y publicaciones de topics
rospy.init_node('conversion', anonymous=True)
pub = rospy.Publisher('/UR3_1/inputs/move_pose', FM, queue_size=10)
rospy.Subscriber("UR3_1/outputs/pose", FM, callback1)
rospy.Subscriber("configuracion", String, callback2)
rospy.Subscriber("movimiento", FM, callback3)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
global iteracion
#Si no hemos recibido ningun dato de movimiento no movemos el robot
if (iteracion == True):
print("NUEVO MOVIMIENTO")
iteracion = False
global j, ro, longitud
Dt = longitud
#-------------Vamos a transformar la orientacion recibida del robot--------------------
#-------------en otra forma de orientacion que podemos manejar mas adelante------------
#Primero calculamos el ANGULO y el EJE del vector de orientacion que recibimos
angle = math.sqrt(ox * ox + oy * oy + oz * oz)
axis = [ox / angle, oy / angle, oz / angle]
#Creamos una matriz de orientacion a partir del angulo y el eje anteriores
a = np.hstack((axis, (angle, )))
Rm = matrix_from_axis_angle(a)
#Obtenemos el Eje Z de nuestra matriz, que es el que necesitamos para los calculos
z = Rm[2]
#Guardamos la posicion del robot en la variable P
P = [xi, yi, zi]
#-------------Una vez tenemos posicion y orientacion vamos a calcular la nueva--------------------
#-------------posicion y orientacion segun el movimiento que queremos para la herramienta------------
#Para la primera vez que realizamos las operaciones vamos a tomar
#una serie de valores determinados
if (j == 0):
fi = float(
fulcro / Dt
) #Calculamos fi con el valor inicial recibido por la interfaz
Pf = P + fi * Dt * z #El punto de fulcro lo calculamos al principio y no cambia
print(Pf)
#La posicion inicial de la herramienta se calcula a partir de los valores de posicion del robot mas la orientacion
Pt = P + Dt * z
#Nueva posicion de la punta con el incremento del Phantom
Ptn = [Pt[0] + Ph1, Pt[1] + Ph2, Pt[2] + Ph3]
#Nueva direccion en el eje z de la herramienta
zn = Pf - Ptn
#Distancia del punto de fulcro a la nueva posicion de la pinza
Mzn = math.sqrt(zn[0] * zn[0] + zn[1] * zn[1] + zn[2] * zn[2])
ro = Dt - Mzn
#Nueva posicion del efector final del robot
Pn = Pf + ro * zn / Mzn
# El eje Z ya lo tnemos, pero lo hacemos unitario
znn = [-zn[0] / Mzn, -zn[1] / Mzn, -zn[2] / Mzn]
#-------------Para calcular la orientacion vamos a calcular los angulos de Euler--------------------
#-------------a partir del eje Z de la matriz, el cual ya tenemos, y una vez tengamos---------------
#-------------los amgulos, llamamos a la funcion que nos calcula la matriz de orientacion-----------
b = math.atan2(math.sqrt(znn[0]**2 + znn[1]**2), znn[2])
a = 0
g = math.atan2((znn[1] / math.sin(b)), (-znn[0] / math.sin(b)))
M = eulerZYZ([a, b, g])
#Pasamos la orientacion a axis-angle, que es la que entiende nuestro simulador
a = axis_angle_from_matrix(M)
orientacion = [a[0] * a[3], a[1] * a[3], a[2] * a[3]]
r = Pn - Ptn
print("Incremento", [Ph1, Ph2, Ph3])
print(" ")
print('Nueva posicion efector Pn', Pn)
print('Nueva posicion herramienta Ptn', Ptn)
print(' ')
print("Comprobaciones")
print(' ')
print('Actual posicion herramienta Pt', Pt)
print('Longitud herramienta Dt',
math.sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]))
print(' ')
print(' ')
#Creamos la variable que contiene la posicion y orientacion que le vamos a dar a nuestro robot
pose = FM()
pose.data = [
Pn[0], Pn[1], Pn[2], orientacion[0], orientacion[1],
orientacion[2]
]
#pose.data = [Pn[0], Pn[1], Pn[2], ox, oy, oz]
#Publicamos la pose
pub.publish(pose)
#Aumentamos la iteracion
j = j + 1
rate.sleep()
rospy.spin()
return copiaImg
def callback(img):
np_arr = np.fromstring(img.data, np.uint8)
imgCV = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
imgCV = cv2.flip(imgCV, 2)
imgCV = acharCirculos(imgCV)
cv2.namedWindow('SHOW_DE_BOLA', cv2.WINDOW_NORMAL)
cv2.imshow('SHOW_DE_BOLA', imgCV)
cv2.waitKey(1)
def listenerImg():
rospy.init_node('showAllCircles', anonymous=True)
rospy.Subscriber('/raspicam_node/image/compressed', CompressedImage,
callback)
rospy.spin()
if __name__ == "__main__":
circulo = BoolStamped()
arrayCoordenadas = Float64MultiArray()
arrayCoordenadas.data = [0, 0, 0]
listenerImg()
# timing
Ts = 0.008
rate = rospy.Rate(125)
# handle waypoints
waypoints = np.array(commands['waypoints'])
duration = waypoints[-1, 0]
t_array = np.arange(0, duration, Ts)
x_array = np.interp(t_array, waypoints[:, 0], waypoints[:, 1])
y_array = np.interp(t_array, waypoints[:, 0], waypoints[:, 2])
setpoints = np.zeros(3)
setpoints_publisher = rospy.Publisher("/hybrid_force_controller/setpoints",
Float64MultiArray,
queue_size=1)
index = 0
while (not rospy.is_shutdown() and index < t_array.shape[0]):
setpoints[0] = x_array[index]
setpoints[1] = y_array[index]
setpoints[2] = desired_force
setpoints_msg = Float64MultiArray()
setpoints_msg.data = setpoints
setpoints_publisher.publish(setpoints_msg)
index += 1
rate.sleep()
def choose_behaviors(number):
global right_pub, left_pub, head_pub, emotionShow_pub, gesturePlay_pub, speechSay_pub, audioPlay_pub
#talking:1~6
if (number == 1):
#show_both_hands:9s
emotionShow_pub.publish("QT/talking")
gesturePlay_pub.publish("numbergame/talking1")
rospy.sleep(9)
elif (number == 2):
#stretch_talk:8s
gesturePlay_pub.publish("numbergame/talking2")
emotionShow_pub.publish("QT/talking")
rospy.sleep(8)
elif (number == 3):
#challenge:5s
gesturePlay_pub.publish("QT/challenge")
emotionShow_pub.publish("QT/talking")
time.sleep(5)
elif (number == 4):
#show left and right:10s
emotionShow_pub.publish("QT/talking")
gesturePlay_pub.publish("numbergame/talking3")
rospy.sleep(10)
elif (number == 5):
#teaching,10s
emotionShow_pub.publish("QT/talking")
gesturePlay_pub.publish("numbergame/talking4")
rospy.sleep(10)
elif (number == 6):
#teaching:10s
emotionShow_pub.publish("QT/talking")
gesturePlay_pub.publish("numbergame/talking5")
rospy.sleep(10)
elif (number == 7):
#show:9s
emotionShow_pub.publish("QT/talking")
gesturePlay_pub.publish("numbergame/talking6")
rospy.sleep(9)
#listening:8~9
elif (number == 8):
#nod:4s
head = Float64MultiArray()
emotionShow_pub.publish("QT/showing_smile")
head.data = [0, -10]
head_pub.publish(head)
time.sleep(1)
head.data = [0, 10]
head_pub.publish(head)
time.sleep(1)
head.data = [0, 0]
head_pub.publish(head)
time.sleep(2)
elif (number == 9):
#arm back smile:7s
emotionShow_pub.publish("QT/calming_down")
time.sleep(1)
gesturePlay_pub.publish("QT/bored")
time.sleep(6)
#guessing:10~12
elif (number == 10):
#confused:11s
emotionShow_pub.publish("QT/confused")
gesturePlay_pub.publish("numbergame/thinking1")
rospy.sleep(11)
elif (number == 11):
#touch head:11s
emotionShow_pub.publish("QT/confused")
gesturePlay_pub.publish("numbergame/thinking2")
rospy.sleep(11)
elif (number == 12):
#thinking:11s
emotionShow_pub.publish("QT/confused")
gesturePlay_pub.publish("numbergame/thinking3")
rospy.sleep(11)
#feedback and encouragement:13~16
elif (number == 13):
#surprise:5.5s
gesturePlay_pub.publish("QT/surprise")
emotionShow_pub.publish("QT/surprise")
time.sleep(5.5)
elif (number == 14):
#happy:5s
gesturePlay_pub.publish("QT/happy")
emotionShow_pub.publish("QT/happy")
time.sleep(5)
elif (number == 15):
#hug:6s
left_arm = Float64MultiArray()
right_arm = Float64MultiArray()
emotionShow_pub.publish("QT/happy")
left_arm.data = [-20, -10, -15]
left_pub.publish(left_arm)
right_arm.data = [20, -10, -15]
right_pub.publish(right_arm)
time.sleep(3)
left_arm.data = [90, -60, -30]
left_pub.publish(left_arm)
right_arm.data = [-90, -60, -30]
right_pub.publish(right_arm)
time.sleep(3)
elif (number == 16):
#hand clap:8.8s
left_arm = Float64MultiArray()
right_arm = Float64MultiArray()
emotionShow_pub.publish("QT/happy")
left_arm.data = [10, -90, -30]
left_pub.publish(left_arm)
right_arm.data = [-10, -90, -30]
right_pub.publish(right_arm)
time.sleep(1.8)
left_arm.data = [10, -90, -90]
left_pub.publish(left_arm)
right_arm.data = [-10, -90, -90]
right_pub.publish(right_arm)
time.sleep(1)
left_arm.data = [10, -90, -30]
left_pub.publish(left_arm)
right_arm.data = [-10, -90, -30]
right_pub.publish(right_arm)
time.sleep(1)
left_arm.data = [10, -90, -90]
left_pub.publish(left_arm)
right_arm.data = [-10, -90, -90]
right_pub.publish(right_arm)
time.sleep(1)
left_arm.data = [10, -90, -30]
left_pub.publish(left_arm)
right_arm.data = [-10, -90, -30]
right_pub.publish(right_arm)
time.sleep(1)
left_arm.data = [90, -60, -30]
left_pub.publish(left_arm)
right_arm.data = [-90, -60, -30]
right_pub.publish(right_arm)
time.sleep(3)
#special_function:17~19
elif (number == 17):
#hi/bye:7s
gesturePlay_pub.publish("QT/hi")
emotionShow_pub.publish("QT/happy")
time.sleep(7)
elif (number == 18):
#fly kiss:7.5s
gesturePlay_pub.publish("QT/kiss")
time.sleep(1)
emotionShow_pub.publish("QT/kiss")
time.sleep(6.5)
elif (number == 19):
#yawn:6.8s
gesturePlay_pub.publish("QT/yawn")
time.sleep(0.8)
emotionShow_pub.publish("QT/yawn")
time.sleep(6)
#rest
elif (number == 20):
abc = 1
else:
print("Please use a correct number!")
def update_state(self):
self.state = np.array(self.im_data+self.imu_data)
self.state_pub.publish(Float64MultiArray(data=self.state))
def send_cmd(self, command): #command = Float64MultiArray
next_joint_state = Float64MultiArray()
next_joint_state.data = command
self.joint_cmd_pub.publish(next_joint_state)
def talker():
print('starting node')
# Publishers
# here I am creating the publisher, you need to insert the topic name of the submarine
pub = rospy.Publisher('/g500/thrusters_input',
Float64MultiArray,
queue_size=10)
# since this is a node, it needs to have a name
rospy.init_node('talker', anonymous=False)
rate = rospy.Rate(10) # 10hz
# Now here I create a message, move odometry, from the twiststamped type of message
msg = Float64MultiArray()
msg.data = [0, 0, 0, 0, 0]
# Reference velocity
v_ref = 0.5 # m/s
# Initialization values
Kp = 1
Ti = 1
Td = 0.001
dt = 0.1
k1 = Kp * (1 + Td / dt)
k2 = -Kp * (1 + 2 * Td / dt - dt / Ti)
k3 = Kp * (Td / Ti)
v0 = 0
error_v0 = 0
error_v1 = 0
error_v2 = 0
rospy.Subscriber('/g500/camera1', Image, callback, queue_size=1)
rospy.Subscriber('/g500/dvl', DVL, callback_dvl, queue_size=1)
while not rospy.is_shutdown():
# for now I dont pay attention to the camera
print('velocity in x', vel_v)
error_v2 = error_v1
error_v1 = error_v0
error_v0 = v_ref - vel_v
control_v = v0 + k1 * error_v0 + k2 * error_v1 + k3 * error_v2
control_v = limit(control_v, 1.0)
msg.data[0] = control_v
msg.data[1] = control_v
v0 = control_v
pub.publish(msg)
rate.sleep()
# when rospy has been shutdown (ctrl + c )stop turtlebot
rospy.loginfo("Stop")
print('Stopping node')
# a default Twist has linear.x of 0 and angular.z of 0. So it'll stop TurtleBot
pub.publish(msg)
# sleep just makes sure TurtleBot receives the stop command prior to shutting down the script
rospy.sleep(1)
print('node stopped')
securitySpeed_publisher.publish(msg)
if __name__ == '__main__':
# x = {}
# y = {}
# z = {}
# securitySpeed = {}
# all_drones = ['cf1', 'cf2', 'cf3', 'cf4', 'cf5', 'cf6']
for drone in all_drones:
x[drone] = 0
y[drone] = 0
z[drone] = 0
securitySpeed[drone] = 0
# global securitySpeed, msg, x1, x2, x3, y1, y2, y3, z1, z2, z3, x4, x5, x6, y4, y5, y6, z4, z5, z6
# x1, x2, x3, y1, y2, y3, z1, z2, z3 = 1000, 100, 10, 1000, 100, 10, 1000, 100, 10
# x4, x5, x6, y4, y5, y6, z4, z5, z6 = 1000, 100, 10, 1000, 100, 10, 1000, 100, 10
# securitySpeed= [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
msg = Float64MultiArray()
rospy.init_node('security', anonymous=True)
securitySpeed_publisher = rospy.Publisher('/security_speed',
Float64MultiArray,
queue_size=10)
cf2_subscriber = rospy.Subscriber('/tf', TFMessage, cf_callback)
rospy.spin()
def __init__(self):
#CREACION DE BROADCAST
self.pub_tf = rospy.Publisher("/tf",
tf2_msgs.msg.TFMessage,
queue_size=5)
self.broadcts = tf2_ros.TransformBroadcaster()
self.transform = TransformStamped()
#SUBSCRIPCION AL TOPICO DE COMUNICACION ENTRE TF_NODE Y CONTROLADOR_NODE
rospy.Subscriber("/next_coord", Bool, self.callback_next)
#rospy.Subscriber("/vel_robot",numpy_msg(Twist),self.callback_position)
rospy.Subscriber("/rover/gps/pos", NavSatFix, self.callback_gps)
rospy.Subscriber("/rover/imu", Imu, self.callback_imu)
self.pub1 = rospy.Publisher("/goal", Twist, queue_size=10)
#PARAMETROS
self.theta = -rospy.get_param("/theta") * 3.141592 / 180.0
self.ds = rospy.get_param("/ds")
self.f = rospy.get_param("/f")
self.coordenadas1 = rospy.get_param("/coordenadas")
self.position = Float64MultiArray()
self.pos_x = 0
self.pos_y = 0
self.theta = 0
self.first_gps = 1
self.pos_x_init = 0
self.pos_y_init = 0
self.angles_odom_base = [0, 0, 0, 0]
self.angles_goal_odom = [0, 0, 0, 0]
self.Meta = Twist()
self.theta_z = 0
while self.first_gps:
self.i = 0 #VARIABLE PARA RECORRER MATRIZ SELF.TRAY (COORDENADAS DE TRAYECTORIA EN EL SISTEMA DEL ROBOT)
self.update_coor(
) ##Actualizacion de coordenadas y creacion de trayectoria
self.evadir_mpi()
self.creacion_tray()
#rospy.loginfo("----------------------------------------------")
#rospy.loginfo(self.coordenadas_new)
#rospy.loginfo("----------------------------------------------")
#rospy.loginfo(self.tray)
rate = rospy.Rate(self.f)
rospy.loginfo("Nodo TF inicio correctamente ")
while (not rospy.is_shutdown()):
if (self.i < len(self.tray) - 1):
self.update_odom() ## Se envia odometria y meta en tf
self.update_goal(self.i + 1)
self.Meta.linear.x = self.tray[self.i + 1][0]
self.Meta.linear.y = self.tray[self.i + 1][1]
self.Meta.linear.z = 0.0
self.Meta.angular.x = 0.0
self.Meta.angular.y = 0.0
self.Meta.angular.z = self.tray[self.i + 1][2]
self.pub1.publish(self.Meta)
rate.sleep()
class TestSequence:
command = Float64MultiArray()
command.layout = MultiArrayLayout()
command.layout.dim = [MultiArrayDimension()]
iterator = 1 #step through commands in the sequence - 0 should be reserved for takeoff
def __init__(self, NUM_UAV):
global status
self.command.layout.dim[0].label = 'command'
self.command.layout.dim[0].size = 4
self.command.layout.dim[0].stride = 4 * 8
self.command.layout.data_offset = 0
#testing circle data = [x, y, r, n]; n - sequence number
self.command.data = [0, 0, 10, 0]
status = [Int8() for i in range(NUM_UAV)]
sum_stat = Int8()
sum_stat.data = 0
# subscribing to each uav's status
for i in range(NUM_UAV):
exec('def status_cb' + str(i) + '(msg): status[' + str(i) +
'] = msg')
#exec ('def state_cb' + str(i) + '(msg): state[' + str(i) + '] = msg' )
#rospy.Subscriber('/mavros/state')
rospy.Subscriber('/sequencer/status' + str(i),
Int8,
callback=eval('status_cb' + str(i)))
rospy.init_node('sequencer_test', anonymous=True)
print "INIT\n"
pub = rospy.Publisher('/sequencer/command',
Float64MultiArray,
queue_size=10)
rospy.Subscriber('/sequencer/status',
Float64MultiArray,
callback=self.status_cb)
rate = rospy.Rate(10) # Hz
rate.sleep()
# wait to all drones to connect to the sequencer
nc = pub.get_num_connections()
while nc < NUM_UAV:
nc = pub.get_num_connections()
rate.sleep()
print 'num_connections = ' + str(nc)
print 'publishing - \n' + str(self.command)
pub.publish(self.command)
sys.stdout.flush()
while not rospy.is_shutdown():
publish = True
for i in range(NUM_UAV):
if status[i].data != self.command.data[3]:
publish = False
if publish and self.iterator < 5:
self.command.data = self.nextCommand()
pub.publish(self.command)
sys.stdout.flush()
def status_cb(self, msg):
self.status = msg
def nextCommand(self):
if self.iterator == 4: #flip to velocity control
temp = [0, 0, 30, self.iterator]
elif self.iterator == 3:
temp = [0, 0, 30, self.iterator]
elif self.iterator == 2:
temp = [0, 0, 40, self.iterator]
elif self.iterator == 1:
temp = [0, 0, 10, self.iterator]
self.iterator += 1
return temp
def __init__(self, parent=None):
super(Thread_transmit_joint_data, self).__init__(parent)
self.eds_file = rp.get_path(
'canopen_communication') + "/file/Copley.eds"
#print path.exists(eds_file)
# 用于接收逆解
# this publisher was to sent desacrtes points by inverse algorithm to getting the joint datas.
self.pub_wdc = rospy.Publisher('climboI5d_Descartes_point',
Float64MultiArray,
queue_size=5)
# this subscriber was to receive the joint data.
self.sub_wdc = rospy.Subscriber('climboI5d_Inverse_solution',
Float64MultiArray,
self.Inverse_solution_callback)
self.descartes_point = Float64MultiArray()
self.inverse_solution = Float64MultiArray()
self.descartes_point.data = [1] * 18
self.inverse_solution.data = [1] * 10
# 关节数据
self.__G0_data = 0.0
self.__I1_data = 0.0
self.__T2_data = 0.0
self.__T3_data = 0.0
self.__T4_data = 0.0
self.__I5_data = 0.0
self.__G6_data = 0.0
#判断是否到达
self.__I1_reached = False
self.__T2_reached = False
self.__T3_reached = False
self.__T4_reached = False
self.__I5_reached = False
# 设置零点使用
self.__I1_error = 0
self.__T2_error = 0
self.__T3_error = 0
self.__T4_error = 0
self.__I5_error = 0
# 用于判断关节是否为新值
self.__G0_old_data = 0.0
self.__I1_old_data = 0.0
self.__T2_old_data = 0.0
self.__T3_old_data = 0.0
self.__T4_old_data = 0.0
self.__I5_old_data = 0.0
self.__G6_old_data = 0.0
# 关节速度
self.__I1_velocity = 0
self.__T2_velocity = 0
self.__T3_velocity = 0
self.__T4_velocity = 0
self.__I5_velocity = 0
# 用于判断关节速度是否为新值
self.__I1_old_velocity = 0
self.__T2_old_velocity = 0
self.__T3_old_velocity = 0
self.__T4_old_velocity = 0
self.__I5_old_velocity = 0
# 末端笛卡尔点初始姿态
self.__Descartes_X = 0.5864
self.__Descartes_Y = 0
self.__Descartes_Z = 0
self.__Descartes_RX = 0
self.__Descartes_RY = 0
self.__Descartes_RZ = 3.141592654
# 末端笛卡尔点初始速度
self.__X_velocity = 0
self.__Y_velocity = 0
self.__Z_velocity = 0
self.__RX_velocity = 0
self.__RY_velocity = 0
self.__RZ_velocity = 0
# 运行模式
self.__position_mode = False
self.__velocity_mode = False
self.__gripper_mode = False
# 停止方式
self.stop = False
self.quick_stop = False
# 判断采用哪种反馈
self.__feedback_joint = False
self.__feedback_descartes = False
self.__feedback_offline = False
# 离线数据索引
self.__index = 0
# 存储末端笛卡尔点位置,速度
self.__descartes_data = []
# 存储离线数据
self.__offline_data = []
# 关节误差
self.__error_joint = 4e-3
def dodge_maneuver():
global scan_rectangle_R
global scan_rectangle_F
global scan_rectangle_L
global twist
global cmd_vel_pub
right_flag = True
right_count = 0
left_flag = True
left_count = 0
obstacle_edge_flag = True
min_max_arr = Float64MultiArray()
min_max_topic = "/range_min_max"
min_max_arr = rospy.wait_for_message(min_max_topic, Float64MultiArray)
minimum, minimum_range = min_max_arr.data[0], min_max_arr.data[1]
robot_center, robot_center_range = min_max_arr.data[2], min_max_arr.data[3]
maximum, maximum_range = min_max_arr.data[4], min_max_arr.data[5]
print('minimum : ', minimum)
print('minimum_range : ', minimum_range)
print('maximum : ', maximum)
print('maximum_range : ', maximum_range)
scan_rectangle_R_topic = "/scan_rectangle_R"
scan_rectangle_F_topic = "/scan_rectangle_F"
scan_rectangle_L_topic = "/scan_rectangle_L"
rospy.Subscriber(scan_rectangle_R_topic, Float64MultiArray,
scan_rectangle_R_send)
rospy.Subscriber(scan_rectangle_F_topic, Float64MultiArray,
scan_rectangle_F_send)
rospy.Subscriber(scan_rectangle_L_topic, Float64MultiArray,
scan_rectangle_L_send)
theta, center = maximum - minimum, (maximum + minimum) / 2
center_2_right = minimum_range * math.sin(
math.radians(robot_center - minimum))
center_2_left = maximum_range * math.sin(
math.radians(maximum - robot_center))
# if center_2_left < 0 :
# center_2_left = 0
# if center_2_right < 0 :
# center_2_right = 0
obstacle_range = math.sqrt(minimum_range**2 + maximum_range**2 -
2 * minimum_range * maximum_range *
math.cos(math.radians(theta)))
print("center : ", center)
print("obstacle_range : ", obstacle_range)
print("center_2_right : ", center_2_right)
print("center_2_left : ", center_2_left)
for x in scan_rectangle_R:
right_count = right_count + 1 if x <= 1 else right_count
right_flag = False if right_count > 15 else True
for x in scan_rectangle_L:
left_count = left_count + 1 if x <= 1 else left_count
left_flag = False if left_count > 15 else True
rotation = -1 if right_flag == True and left_flag == False else +1 if left_flag == True and right_flag == False else 9999 if right_flag == False and left_flag == False else 0
if rotation == 0:
if center_2_left >= center_2_right:
horizontal_time = (center_2_right + 0.5) / 0.2
rotation = -1
else:
horizontal_time = (center_2_left + 0.5) / 0.2
rotation = 1
elif rotation == 1:
horizontal_time = (center_2_left + 0.5) / 0.2
elif rotation == -1:
horizontal_time = (center_2_right + 0.5) / 0.2
elif rotation == 9999:
return 0
vertical_time = 4
original_goal_z = robot_z
goal_z = robot_z + (rotation * 90)
if goal_z >= 360:
goal_z = goal_z - 360
elif goal_z < 0:
goal_z = 360 + goal_z
for i in range(1, 8):
if i == 1 or i == 3 or i == 5 or i == 7:
if i == 1:
x = 1
elif i == 3:
x = -1
goal_z = original_goal_z
elif i == 5:
goal_z = z + (rotation * (-90))
if goal_z >= 360:
goal_z = goal_z - 360
elif goal_z < 0:
goal_z = 360 + goal_z
x = -1
elif i == 7:
x = 1
goal_z = original_goal_z
print("goal_z")
while (abs(goal_z - robot_z) >= 2): #1 rotate
twist.angular.z = a_val * rotation * x
cmd_vel_pub.publish(twist)
elif i == 2 or i == 4 or i == 6:
if i == 2:
go_time = horizontal_time
print("horizontal_time : ", horizontal_time)
elif i == 4:
go_time = vertical_time
if vertical_go(rotation) == 1:
return 1
print("vertical_time : ", vertical_time)
elif i == 6:
go_time = horizontal_time
print("horizontal_time : ", horizontal_time)
start = time.time()
while (time.time() - start <= go_time): #1 go
if warning_msg.warning[1] == 'front':
print("front_again")
twist_init()
cmd_vel_pub.publish(twist)
return 1
twist.linear.x = x_val
cmd_vel_pub.publish(twist)
twist_init()
cmd_vel_pub.publish(twist)
rospy.sleep(1)
return 0
import cv2.cv as cv
# initialize the camera and grab a reference to the raw camera capture
camera = PiCamera()
camera.resolution = (160,120)
camera.framerate = 32
rawCapture = PiRGBArray(camera, size=(160,120))
# allow the camera to warm up
time.sleep(0.1)
# initialize publisher
rospy.init_node('ball_position',anonymous=True)
pub = rospy.Publisher('ball_pos',Float64MultiArray,queue_size = 10)
ball_pos = Float64MultiArray()
flag=1
for frame in camera.capture_continuous(rawCapture, format='bgr', use_video_port=True):
image = frame.array
if flag:
# setup initial location of window
r,h,c,w = 80-40,80+40,64-30,64+30 # simply hardcoded the values
track_window = (c,r,w,h)
# set up the ROI for tracking
hsv_roi = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
# define range of blue color in HSV
#!/usr/bin/env python
import rospy
from std_msgs.msg import Float64MultiArray
from my_pkg.msg import Topic1
import numpy as np
result = Float64MultiArray()
def quaternion_to_euler(w, x, y, z):
sinr_cosp = 2 * (w * x + y * z)
cosr_cosp = 1 - 2 * (x**2 + y**2)
roll = np.arctan2(sinr_cosp, cosr_cosp)
sinp = 2 * (w * y - z * x)
pitch = np.where(
np.abs(sinp) >= 1,
np.sign(sinp) * np.pi / 2, np.arcsin(sinp))
siny_cosp = 2 * (w * z + x * y)
cosy_cosp = 1 - 2 * (y**2 + z**2)
yaw = np.arctan2(siny_cosp, cosy_cosp)
return roll, pitch, yaw
def callback(msg):
result.data = quaternion_to_euler(msg.x, msg.y, msg.z, msg.w)
rospy.loginfo(result)
def callback(data):
a = np.reshape(data.data,(2048,2048))
setx = [[] for ii in range(10)]
sety = [[] for ii in range(10)]
setall = [[] for ii in range(10)]
X = len(a)-1
Y = len(a[0])-1
neighbors = lambda x, y : [(x2, y2) for x2 in range(x-5, x+6)
for y2 in range(y-5, y+6)
if (-1 < x <= X and
-1 < y <= Y and
(x != x2 or y != y2) and
(0 <= x2 <= X) and
(0 <= y2 <= Y))]
for rep in range(5):
pointx = start[0]
pointy = start[1]
X = len(a)-1
Y = len(a[0])-1
while (pointx != stop[0])|(pointy!=stop[1]):
n = neighbors(pointx,pointy)
p = random.choice(n)
wcount = 0
while((((abs(stop[0]-p[0]))>(abs(stop[0]-pointx)))&
((abs(stop[1]-p[1]))>(abs(stop[1]-pointy))))|
(a[p[0]][p[1]]!=0)|
((lookup(p[0],p[1],a) == 0)&
(lookdown(p[0],p[1],a) == 0)&
(lookleft(p[0],p[1],a) == 0)&
(lookright(p[0],p[1],a) == 0))|
([p[0],p[1]] in setall)):
p = random.choice(n)
wcount+=1
if wcount == 300:
break
if (stop[0],stop[1]) in n:
p = (stop[0],stop[1])
setx[rep].append(p[0])
sety[rep].append(p[1])
setall[rep].append([p[0],p[1]])
pointx = p[0]
pointy = p[1]
if ((len(setall[rep])>2000)|(wcount==300)):
setx[rep] = []
sety[rep] = []
setall[rep] = []
pointx = start[0]
pointy = start[1]
X = len(a)-1
Y = len(a[0])-1
ptr = 1
while (ptr<len(setall[rep])-1):
kk = neighbors(setx[rep][ptr-1],sety[rep][ptr-1])
if (((sqrt((abs(setx[rep][ptr+1] - setx[rep][ptr-1])^2) +
((abs(sety[rep][ptr+1] - sety[rep][ptr-1])^2)))<
(sqrt((abs(setx[rep][ptr] - setx[rep][ptr-1])^2) +
((abs(sety[rep][ptr] - sety[rep][ptr-1])^2)))))|
((setx[rep][ptr+1],sety[rep][ptr+1]) in kk))|
(a[((setx[rep][ptr+1]-setx[rep][ptr-1])/2)+setx[rep][ptr-1]]
[((sety[rep][ptr+1]-sety[rep][ptr-1])/2)+sety[rep][ptr-1]] == 0)):
del setx[rep][ptr]
del sety[rep][ptr]
del setall[rep][ptr]
ptr+=1
else:
ptr+=1
pos = 0
for rep in range(5):
if(len(setx[rep])<len(setx[pos])):
pos = rep
arr = Float64MultiArray()
rate = rospy.Rate(100) # 10hz
sety[pos]= scipy.ndimage.gaussian_filter(sety[pos],6)
setx[pos] = scipy.ndimage.gaussian_filter(setx[pos],10)
# fig, ax = plt.subplots()
# ax.plot(sety[pos], setx[pos])
# ax.imshow(a)
# plt.show()
setsend = [[0,0]for ii in range(len(setx[pos]))]
for i in range(len(setx[pos])):
setsend[i] = [setx[pos][i],sety[pos][i]]
arr.data = np.ndarray.flatten(np.array(setsend)).tolist()
for i in range(len(arr.data)):
arr.data[i] -= 1024
arr.data[i]/=scale
pub.publish(arr)
print arr
exit(0)
def spin(self):
# Prob. Grid initialisation constant
prob_init_const = 0.01
rospy.loginfo("Prob_Grid_Updater: Starting Spin function")
r = rospy.Rate(1)
while not rospy.is_shutdown():
cv_xdim = self.r0_current_vision.layout.dim[1].size
cv_ydim = self.r0_current_vision.layout.dim[0].size
new_prob_grids = Float64MultiArray()
new_prob_grids.layout.dim.extend([
MultiArrayDimension(),
MultiArrayDimension(),
MultiArrayDimension()
])
new_prob_grids.layout.dim[0].size = 3
new_prob_grids.layout.dim[0].label = "grids"
new_prob_grids.layout.dim[1].label = "rows"
new_prob_grids.layout.dim[1].size = cv_ydim
new_prob_grids.layout.dim[2].label = "cols"
new_prob_grids.layout.dim[2].size = cv_xdim
new_prob_grids.data = []
# Assign new / old times
if self.first_time == True:
rospy.loginfo(
"Prob_Grid_Updater: First time so initialising prob grids")
self.last_update_time = rospy.get_time()
# init the CP grids
# data initialisations
# Expected and CP grid uniformly as 0
for i in range(0, 3):
for j in range(0, cv_ydim):
for k in range(0, cv_xdim):
if (i == 0):
new_prob_grids.data.append(prob_init_const)
else:
new_prob_grids.data.append(0)
self.first_time = False
else:
self.current_time = rospy.get_time()
# rospy.loginfo("Prob_Grid_Updater: Current Time = %i", self.current_time)
expected_list = self.update_exp_grid()
prob_list = self.update_prob_grid()
cp_list = self.update_cp_grid()
new_prob_grids.data.extend(prob_list)
new_prob_grids.data.extend(cp_list)
new_prob_grids.data.extend(expected_list)
# self.print_list(prob_list,20)
# print(" ")
# self.print_list(expected_list, 20)
# print(" ")
# self.print_list(cp_list, 20)
self.publish_to_topics(new_prob_grids)
self.last_update_time = self.current_time
# rospy.loginfo('Updated Prob_Grids')
r.sleep()
def callback2(self, data):
# Recieve the image
try:
self.image2 = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
#Blob detection-------------------------------------------------------
def detect_red(self, image1, image2):
#smooth the image and reduce noise
image_gau_blur1 = cv2.GaussianBlur(image1, (1, 1), 0)
#convert colours to HSV
hsv1 = cv2.cvtColor(image_gau_blur1, cv2.COLOR_BGR2HSV)
#set the HSV values for red
lower_red1 = np.array([0, 200, 0])
higher_red1 = np.array([0, 255, 255])
#Apply threshold to seperate the blob from rest of the robot
red_range1 = cv2.inRange(hsv1, lower_red1, higher_red1)
res_red1 = cv2.bitwise_and(image_gau_blur1,
image_gau_blur1,
mask=red_range1)
#convert to grey scale
red_s_gray1 = cv2.cvtColor(res_red1, cv2.COLOR_BGR2GRAY)
#Detect the edges
canny_edge1 = cv2.Canny(red_s_gray1, 30, 70)
#Find the contours
contours1, hierarchy1 = cv2.findContours(canny_edge1,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
#Find the center coordinates and the radius of the blob
(x1, y1), radius1 = cv2.minEnclosingCircle(contours1[0])
#convert to integers
cy, cz1 = (int(x1), int(y1))
radius1 = int(radius1)
#similar to above, but for image 2
image_gau_blur2 = cv2.GaussianBlur(image2, (1, 1), 0)
hsv2 = cv2.cvtColor(image_gau_blur2, cv2.COLOR_BGR2HSV)
lower_red2 = np.array([0, 200, 0])
higher_red2 = np.array([0, 255, 255])
red_range2 = cv2.inRange(hsv2, lower_red2, higher_red2)
res_red2 = cv2.bitwise_and(image_gau_blur2,
image_gau_blur2,
mask=red_range2)
red_s_gray2 = cv2.cvtColor(res_red2, cv2.COLOR_BGR2GRAY)
canny_edge2 = cv2.Canny(red_s_gray2, 30, 70)
contours2, hierarchy2 = cv2.findContours(canny_edge2,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
(x2, y2), radius2 = cv2.minEnclosingCircle(contours2[0])
cx, cz2 = (int(x2), int(y2))
radius2 = int(radius2)
return np.array([cx, cy, cz1, cz2])
def detect_blue(self, image1, image2):
#similar approach as detect_blue but different colour threshold
image_gau_blur1 = cv2.GaussianBlur(image1, (1, 1), 0)
hsv1 = cv2.cvtColor(image_gau_blur1, cv2.COLOR_BGR2HSV)
lower_red1 = np.array([70, 0, 0])
higher_red1 = np.array([255, 255, 255])
red_range1 = cv2.inRange(hsv1, lower_red1, higher_red1)
res_red1 = cv2.bitwise_and(image_gau_blur1,
image_gau_blur1,
mask=red_range1)
red_s_gray1 = cv2.cvtColor(res_red1, cv2.COLOR_BGR2GRAY)
canny_edge1 = cv2.Canny(red_s_gray1, 30, 70)
contours1, hierarchy1 = cv2.findContours(canny_edge1,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
(x1, y1), radius1 = cv2.minEnclosingCircle(contours1[0])
cy, cz1 = (int(x1), int(y1))
radius1 = int(radius1)
image_gau_blur2 = cv2.GaussianBlur(image2, (1, 1), 0)
hsv2 = cv2.cvtColor(image_gau_blur2, cv2.COLOR_BGR2HSV)
lower_red2 = np.array([70, 0, 0])
higher_red2 = np.array([255, 255, 255])
red_range2 = cv2.inRange(hsv2, lower_red2, higher_red2)
res_red2 = cv2.bitwise_and(image_gau_blur2,
image_gau_blur2,
mask=red_range2)
red_s_gray2 = cv2.cvtColor(res_red2, cv2.COLOR_BGR2GRAY)
canny_edge2 = cv2.Canny(red_s_gray2, 30, 70)
contours2, hierarchy2 = cv2.findContours(canny_edge2,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
(x2, y2), radius2 = cv2.minEnclosingCircle(contours2[0])
cx, cz2 = (int(x2), int(y2))
radius2 = int(radius2)
return np.array([cx, cy, cz1, cz2])
def detect_green(self, image1, image2):
#similar approach as detect_blue but different colour threshold
image_gau_blur1 = cv2.GaussianBlur(image1, (1, 1), 0)
hsv1 = cv2.cvtColor(image_gau_blur1, cv2.COLOR_BGR2HSV)
lower_red1 = np.array([55, 0, 0])
higher_red1 = np.array([100, 255, 255])
red_range1 = cv2.inRange(hsv1, lower_red1, higher_red1)
res_red1 = cv2.bitwise_and(image_gau_blur1,
image_gau_blur1,
mask=red_range1)
red_s_gray1 = cv2.cvtColor(res_red1, cv2.COLOR_BGR2GRAY)
canny_edge1 = cv2.Canny(red_s_gray1, 30, 70)
contours1, hierarchy1 = cv2.findContours(canny_edge1,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
(x1, y1), radius1 = cv2.minEnclosingCircle(contours1[0])
cy, cz1 = (int(x1), int(y1))
radius1 = int(radius1)
image_gau_blur2 = cv2.GaussianBlur(image2, (1, 1), 0)
hsv2 = cv2.cvtColor(image_gau_blur2, cv2.COLOR_BGR2HSV)
lower_red2 = np.array([55, 0, 0])
higher_red2 = np.array([100, 255, 255])
red_range2 = cv2.inRange(hsv2, lower_red2, higher_red2)
res_red2 = cv2.bitwise_and(image_gau_blur2,
image_gau_blur2,
mask=red_range2)
red_s_gray2 = cv2.cvtColor(res_red2, cv2.COLOR_BGR2GRAY)
canny_edge2 = cv2.Canny(red_s_gray2, 30, 70)
contours2, hierarchy2 = cv2.findContours(canny_edge2,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
(x2, y2), radius2 = cv2.minEnclosingCircle(contours2[0])
cx, cz2 = (int(x2), int(y2))
radius2 = int(radius2)
return np.array([cx, cy, cz1, cz2])
def detect_yellow(self, image1, image2):
#similar approach as detect_blue but different colour threshold
image_gau_blur1 = cv2.GaussianBlur(image1, (1, 1), 0)
hsv1 = cv2.cvtColor(image_gau_blur1, cv2.COLOR_BGR2HSV)
lower_red1 = np.array([16, 244, 0])
higher_red1 = np.array([51, 255, 255])
red_range1 = cv2.inRange(hsv1, lower_red1, higher_red1)
res_red1 = cv2.bitwise_and(image_gau_blur1,
image_gau_blur1,
mask=red_range1)
red_s_gray1 = cv2.cvtColor(res_red1, cv2.COLOR_BGR2GRAY)
canny_edge1 = cv2.Canny(red_s_gray1, 30, 70)
contours1, hierarchy1 = cv2.findContours(canny_edge1,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
(x1, y1), radius1 = cv2.minEnclosingCircle(contours1[0])
cy, cz1 = (int(x1), int(y1))
radius1 = int(radius1)
image_gau_blur2 = cv2.GaussianBlur(image2, (1, 1), 0)
hsv2 = cv2.cvtColor(image_gau_blur2, cv2.COLOR_BGR2HSV)
lower_red2 = np.array([16, 244, 0])
higher_red2 = np.array([51, 255, 255])
red_range2 = cv2.inRange(hsv2, lower_red2, higher_red2)
res_red2 = cv2.bitwise_and(image_gau_blur2,
image_gau_blur2,
mask=red_range2)
red_s_gray2 = cv2.cvtColor(res_red2, cv2.COLOR_BGR2GRAY)
canny_edge2 = cv2.Canny(red_s_gray2, 30, 70)
contours2, hierarchy2 = cv2.findContours(canny_edge2,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
(x2, y2), radius2 = cv2.minEnclosingCircle(contours2[0])
cx, cz2 = (int(x2), int(y2))
radius2 = int(radius2)
return np.array([cx, cy, cz1, cz2])
def detect_blue_contours(image1):
#similar to detect_red(), this one only returns the positions of the contour
image_gau_blur1 = cv2.GaussianBlur(image1, (1, 1), 0)
hsv1 = cv2.cvtColor(image_gau_blur1, cv2.COLOR_BGR2HSV)
lower_red1 = np.array([70, 0, 0])
higher_red1 = np.array([255, 255, 255])
red_range1 = cv2.inRange(hsv1, lower_red1, higher_red1)
res_red1 = cv2.bitwise_and(image_gau_blur1,
image_gau_blur1,
mask=red_range1)
red_s_gray1 = cv2.cvtColor(res_red1, cv2.COLOR_BGR2GRAY)
canny_edge1 = cv2.Canny(red_s_gray1, 30, 70)
contours1, hierarchy1 = cv2.findContours(canny_edge1,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
return np.array([contours1])
def detect_yellow_contours(image1):
#similar to detect_red(), this one only returns the positions of the contour
image_gau_blur1 = cv2.GaussianBlur(image1, (1, 1), 0)
hsv1 = cv2.cvtColor(image_gau_blur1, cv2.COLOR_BGR2HSV)
lower_red1 = np.array([16, 244, 0])
higher_red1 = np.array([51, 255, 255])
red_range1 = cv2.inRange(hsv1, lower_red1, higher_red1)
res_red1 = cv2.bitwise_and(image_gau_blur1,
image_gau_blur1,
mask=red_range1)
red_s_gray1 = cv2.cvtColor(res_red1, cv2.COLOR_BGR2GRAY)
canny_edge1 = cv2.Canny(red_s_gray1, 30, 70)
contours1, hierarchy1 = cv2.findContours(canny_edge1,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
(x1, y1), radius1 = cv2.minEnclosingCircle(contours1[0])
cy, cz1 = (int(x1), int(y1))
return np.array([contours1])
def get_y1_y2(yellow_contours, blue_contours):
#finds the z value at the top of yellow blob
y1 = np.min(yellow_contours, axis=0)
y1 = y1[0][0]
y1 = y1[:, 1]
#finds the z value at the bottom of blue blob
y2 = np.max(blue_contours, axis=0)
y2 = y2[0][0]
y2 = y2[:, 1]
return y1, y2
def pixelTometer(self, image1, image2):
#gets the contour coordinates of the blue and yellow
yellow_contours = detect_yellow_contours(image2)
blue_contours = detect_blue_contours(image2)
#finds the z value of center of mass of blue blob
y2 = detect_blue(self, image1, image2)
y2 = y2[3]
#returns the position of arm 1 ends
y1, y2 = get_y1_y2(yellow_contours, blue_contours)
#get the pixel to meter ratio by dividing arm1 length by pixel distance calculated
p2m = 2.5 / (y1 - y2)
#65 is the best number
return p2m
#----------------------------------------------------------------------------------------------
#Angle Detection starts here
def detect_angles_blob(self, image1, image2):
#Calculate the pixel to meter ratio
try:
p = pixelTometer(self, image1, image2)
self.p2m = p
except Exception as e:
p = self.p2m
#find the positions of the blob
try:
green = detect_green(self, image1, image2)
self.green = green
except Exception as e:
green = self.green
try:
red = detect_red(self, image1, image2)
self.red = red
except Exception as e:
red = self.red
#convert to meters
p = pixelTometer(self, image1, image2)
yellow = detect_yellow(self, image1, image2)
blue = detect_blue(self, image1, image2)
#convert from pixel frame to camera frame on z value
green[2] = 800 - green[2]
yellow[2] = 800 - yellow[2]
red[2] = 800 - red[2]
#get ja1, ja and ja3
ja1 = 0.0
ja3 = get_ja3(green, yellow, p)
ja2 = get_ja2(green, yellow, p, ja3)
try:
green = detect_green(self, image1, image2)
self.green = green
except Exception as e:
green = self.green
try:
red = detect_red(self, image1, image2)
self.red = red
except Exception as e:
red = self.red
yellow = p * detect_yellow(self, image1, image2)
blue = p * detect_blue(self, image1, image2)
#get ja4 value
ja4 = np.arctan2(
(green[2] - red[2]), -(green[1] - red[1])) - np.pi / 2 - ja2
return np.array([ja1, ja2, ja3, ja4])
def angle_trajectory(self):
#the angle coordinates given to the target
curr_time = np.array([rospy.get_time() - self.time_trajectory])
ja1 = 0.1
ja2 = float((np.pi / 2) * np.sin((np.pi / 15) * curr_time))
ja3 = float((np.pi / 2) * np.sin((np.pi / 18) * curr_time))
ja4 = float((np.pi / 2) * np.sin((np.pi / 20) * curr_time))
return np.array([ja1, ja2, ja3, ja4])
def get_ja3(green_posn, yellow_posn, p):
#find the distance between green and yellow
green = green_posn - yellow_posn
#convert the distance to meter
X_green = green[0] * p
#X_green[0] cannot be greater than 3.5 or less than -3.5.
#if the code reads that, it might be a pixel error. Therefore we are forcing the system to assume its max value
if X_green > 3.5:
X_green = 3.5
elif X_green < -3.5:
X_green = -3.5
ja3 = np.arcsin(X_green / 3.5)
return ja3
def get_ja2(green_posn, yellow_posn, p, ja3):
green = green_posn - yellow_posn
Y_green = green[1] * p
#Y_green[0] cannot be greater than 3.5 or less than -3.5.
#if the code reads that, it might be a pixel error. Therefore we are forcing the system to assume its max value
if Y_green[0] > 3.5:
Y_green[0] = 3.5
elif Y_green[0] < -3.5:
Y_green[0] = -3.5
#calculate the value before being supplied into arcsin()
arc_sin_val = np.round(Y_green[0] / (-3.5 * np.cos(ja3)), 2)
#value inside the arcsin() cannot be greater than 1 or less than -1
#if the number is greater or lower, we are focing it to accept the largest possible value
if arc_sin_val > 1:
arc_sin_val = 1
elif arc_sin_val < -1:
arc_sin_val = -1
ja2 = np.arcsin(arc_sin_val)
return ja2
self.joints = Float64MultiArray()
#get the joint angles from computer vision
self.joints.data = detect_angles_blob(self, self.image1, self.image2)
#get the joint angles generated automatically
ja1, ja2, ja3, ja4 = angle_trajectory(self)
self.joint1 = Float64()
self.joint1.data = 0
self.joint2 = Float64()
self.joint2.data = ja2
self.joint3 = Float64()
self.joint3.data = ja3
self.joint4 = Float64()
self.joint4.data = ja4
#print(curr_time)
try:
self.image_pub1.publish(
self.bridge.cv2_to_imgmsg(self.image1, "bgr8"))
self.image_pub2.publish(
self.bridge.cv2_to_imgmsg(self.image2, "bgr8"))
self.joints_pub.publish(self.joints)
self.robot_joint1_pub.publish(self.joint1)
self.robot_joint2_pub.publish(self.joint2)
self.robot_joint3_pub.publish(self.joint3)
self.robot_joint4_pub.publish(self.joint4)
except CvBridgeError as e:
print(e)