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)