From 02f7353b1d4396bc712a51e82e13793ec73487de Mon Sep 17 00:00:00 2001 From: weijian Date: Thu, 25 Aug 2022 12:41:25 +0800 Subject: [PATCH] update code to noetic --- Mybuddy/mybuddy/CMakeLists.txt | 3 - Mybuddy/mybuddy/launch/detect_marker.launch | 16 -- .../launch/detect_marker_with_topic.launch | 29 ---- Mybuddy/mybuddy/launch/test.launch | 4 +- Mybuddy/mybuddy/scripts/detect_marker.py | 123 -------------- Mybuddy/mybuddy/scripts/follow_and_pump.py | 155 ------------------ Mybuddy/mybuddy/scripts/follow_display.py | 76 +-------- Mybuddy/mybuddy/scripts/following_marker.py | 64 -------- Mybuddy/mybuddy/scripts/listen_real.py | 2 +- Mybuddy/mybuddy/scripts/simple_gui.py | 4 +- Mybuddy/mybuddy/scripts/slider_control.py | 6 +- Mybuddy/mybuddy/scripts/teleop_keyboard.py | 2 +- 12 files changed, 18 insertions(+), 466 deletions(-) delete mode 100644 Mybuddy/mybuddy/launch/detect_marker.launch delete mode 100644 Mybuddy/mybuddy/launch/detect_marker_with_topic.launch delete mode 100644 Mybuddy/mybuddy/scripts/detect_marker.py delete mode 100644 Mybuddy/mybuddy/scripts/follow_and_pump.py delete mode 100644 Mybuddy/mybuddy/scripts/following_marker.py diff --git a/Mybuddy/mybuddy/CMakeLists.txt b/Mybuddy/mybuddy/CMakeLists.txt index 7bf316b..1d55465 100644 --- a/Mybuddy/mybuddy/CMakeLists.txt +++ b/Mybuddy/mybuddy/CMakeLists.txt @@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS scripts/teleop_keyboard.py scripts/listen_real.py scripts/listen_real_of_topic.py - scripts/detect_marker.py - scripts/following_marker.py - scripts/follow_and_pump.py scripts/simple_gui.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/Mybuddy/mybuddy/launch/detect_marker.launch b/Mybuddy/mybuddy/launch/detect_marker.launch deleted file mode 100644 index dec163d..0000000 --- a/Mybuddy/mybuddy/launch/detect_marker.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/Mybuddy/mybuddy/launch/detect_marker_with_topic.launch b/Mybuddy/mybuddy/launch/detect_marker_with_topic.launch deleted file mode 100644 index becd0d8..0000000 --- a/Mybuddy/mybuddy/launch/detect_marker_with_topic.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/Mybuddy/mybuddy/launch/test.launch b/Mybuddy/mybuddy/launch/test.launch index f4fd31e..6264a2e 100644 --- a/Mybuddy/mybuddy/launch/test.launch +++ b/Mybuddy/mybuddy/launch/test.launch @@ -6,8 +6,8 @@ - - + + diff --git a/Mybuddy/mybuddy/scripts/detect_marker.py b/Mybuddy/mybuddy/scripts/detect_marker.py deleted file mode 100644 index 04ca483..0000000 --- a/Mybuddy/mybuddy/scripts/detect_marker.py +++ /dev/null @@ -1,123 +0,0 @@ -#!/usr/bin/env python -import rospy -import cv2 as cv -import numpy as np -from cv_bridge import CvBridge, CvBridgeError -from sensor_msgs.msg import Image -import tf -from tf.broadcaster import TransformBroadcaster -import tf_conversions -from mycobot_communication.srv import ( - GetCoords, - SetCoords, - GetAngles, - SetAngles, - GripperStatus, -) - - -class ImageConverter: - def __init__(self): - self.br = TransformBroadcaster() - self.bridge = CvBridge() - self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250) - self.aruo_params = cv.aruco.DetectorParameters_create() - calibrationParams = cv.FileStorage( - "calibrationFileName.xml", cv.FILE_STORAGE_READ - ) - self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat() - self.camera_matrix = None - # subscriber, listen wether has img come in. - self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) - - def callback(self, data): - """Callback function. - - Process image with OpenCV, detect Mark to get the pose. Then acccording the - pose to transforming. - """ - try: - # trans `rgb` to `gbr` for opencv. - cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") - except CvBridgeError as e: - print(e) - size = cv_image.shape - focal_length = size[1] - center = [size[1] / 2, size[0] / 2] - if self.camera_matrix is None: - # calc the camera matrix, if don't have. - self.camera_matrix = np.array( - [ - [focal_length, 0, center[0]], - [0, focal_length, center[1]], - [0, 0, 1], - ], - dtype=np.float32, - ) - gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY) - # detect aruco marker. - ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params) - corners, ids = ret[0], ret[1] - # process marker data. - if len(corners) > 0: - if ids is not None: - # print('corners:', corners, 'ids:', ids) - - # detect marker pose. - # argument: - # marker corners - # marker size (meter) - ret = cv.aruco.estimatePoseSingleMarkers( - corners, 0.05, self.camera_matrix, self.dist_coeffs - ) - (rvec, tvec) = (ret[0], ret[1]) - (rvec - tvec).any() - - print("rvec:", rvec, "tvec:", tvec) - - # just select first one detected marker. - for i in range(rvec.shape[0]): - cv.aruco.drawDetectedMarkers(cv_image, corners) - cv.aruco.drawAxis( - cv_image, - self.camera_matrix, - self.dist_coeffs, - rvec[i, :, :], - tvec[i, :, :], - 0.03, - ) - - xyz = tvec[0, 0, :] - xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03] - - # get quaternion for ros. - euler = rvec[0, 0, :] - tf_change = tf.transformations.quaternion_from_euler( - euler[0], euler[1], euler[2] - ) - print("tf_change:", tf_change) - - # trans pose according [joint1] - self.br.sendTransform( - xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange" - ) - - # [x, y, z, -172, 3, -46.8] - cv.imshow("Image", cv_image) - - cv.waitKey(3) - try: - pass - except CvBridgeError as e: - print(e) - - -if __name__ == "__main__": - try: - rospy.init_node("detect_marker") - rospy.loginfo("Starting cv_bridge_test node") - ImageConverter() - rospy.spin() - except KeyboardInterrupt: - print("Shutting down cv_bridge_test node.") - cv.destroyAllWindows() diff --git a/Mybuddy/mybuddy/scripts/follow_and_pump.py b/Mybuddy/mybuddy/scripts/follow_and_pump.py deleted file mode 100644 index fa6b0a3..0000000 --- a/Mybuddy/mybuddy/scripts/follow_and_pump.py +++ /dev/null @@ -1,155 +0,0 @@ -#!/usr/bin/env python2 -import rospy -from visualization_msgs.msg import Marker -import time - -from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus - - -rospy.init_node("gipper_subscriber", anonymous=True) -angle_pub = rospy.Publisher("mycobot/angles_goal", MycobotSetAngles, queue_size=5) -coord_pub = rospy.Publisher("mycobot/coords_goal", MycobotSetCoords, queue_size=5) -pump_pub = rospy.Publisher("mycobot/pump_status", MycobotPumpStatus, queue_size=5) - -angles = MycobotSetAngles() -coords = MycobotSetCoords() -pump = MycobotPumpStatus() - -x_offset = -20 -y_offset = 20 -z_offset = 110 - -flag = False - -temp_x = temp_y = temp_z = 0.0 - -temp_time = time.time() - - -def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2): - coords.x = x - coords.y = y - coords.z = z - coords.rx = rx - coords.ry = ry - coords.rz = rz - coords.speed = 70 - coords.model = m - # print(coords) - coord_pub.publish(coords) - - -def pub_angles(a, b, c, d, e, f, sp): - angles.joint_1 = float(a) - angles.joint_2 = float(b) - angles.joint_3 = float(c) - angles.joint_4 = float(d) - angles.joint_5 = float(e) - angles.joint_6 = float(f) - angles.speed = sp - angle_pub.publish(angles) - - -def pub_pump(flag): - pump.Status = flag - pump_pub.publish(pump) - - -def target_is_moving(x, y, z): - count = 0 - for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)): - print(o, n) - if abs(o - n) < 2: - count += 1 - print(count) - if count == 3: - return False - return True - - -def grippercallback(data): - global flag, temp_x, temp_y, temp_z - # rospy.loginfo('gripper_subscriber get date :%s', data) - if flag: - return - - # pump length: 88mm - x = float(format(data.pose.position.x * 1000, ".2f")) - y = float(format(data.pose.position.y * 1000, ".2f")) - z = float(format(data.pose.position.z * 1000, ".2f")) - - if ( - time.time() - temp_time < 30 - or (temp_x == temp_y == temp_z == 0.0) - or target_is_moving(x - x_offset, y - y_offset, z) - ): - - x -= x_offset - y -= y_offset - pub_coords(x - 20, y, 280) - time.sleep(0.1) - - temp_x, temp_y, temp_z = x, y, z - return - else: - print(x, y, z) - - # detect heigth + pump height + limit height + offset - x += x_offset - y += y_offset - z = z + 88 + z_offset - - pub_coords(x, y, z) - time.sleep(2.5) - - # down - for i in range(1, 17): - pub_coords(x, y, z - i * 5, rx=-160, sp=10) - time.sleep(0.1) - - time.sleep(2) - - pub_pump(True) - # pump on - - pub_coords(x, y, z + 20, -165) - time.sleep(1.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - put_z = 140 - pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2) - time.sleep(1.5) - - for i in range(1, 8): - pub_coords(39.4, -174.7, put_z - i * 5, -177.13, -4.13, -152.59, 15, 2) - time.sleep(0.1) - - pub_pump(False) - - time.sleep(0.5) - - pub_angles(0, 30, -50, -40, 0, 0, 50) - time.sleep(1.5) - - # finally - flag = True - - -def main(): - for _ in range(10): - # pub_coords(150, 20, 220, -175, 0, -90, 70, 2) - pub_angles(0, 30, -50, -40, 0, 0, 50) - # pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70) - time.sleep(0.5) - - pub_pump(False) - # time.sleep(2.5) - rospy.Subscriber("visualization_marker", Marker, grippercallback, queue_size=1) - print("gripper test") - rospy.spin() - - -if __name__ == "__main__": - main() diff --git a/Mybuddy/mybuddy/scripts/follow_display.py b/Mybuddy/mybuddy/scripts/follow_display.py index 14e496d..76000bd 100755 --- a/Mybuddy/mybuddy/scripts/follow_display.py +++ b/Mybuddy/mybuddy/scripts/follow_display.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import time import os import rospy @@ -41,12 +41,6 @@ def talker(): joint_state_send.header = Header() joint_state_send.name = [ - # "joint2_to_joint1", - # "joint3_to_joint2", - # "joint4_to_joint3", - # "joint5_to_joint4", - # "joint6_to_joint5", - # "joint6output_to_joint6", "joint1_L", "joint2_L", "joint3_L", @@ -69,94 +63,42 @@ def talker(): marker_.header.frame_id = "/base_link1" marker_.ns = "my_namespace" - print("joint_state_send:%s" % joint_state_send) print("publishing ...") while not rospy.is_shutdown(): joint_state_send.header.stamp = rospy.Time.now() - # =======left_radians======= - # lencoder = mb.get_encoders(1) - # left_radians = [] - # for index, value in enumerate(lencoder): - # value = (value-2048)*2*math.pi/4096 - # left_radians.append(value) - left_radians = mb.get_radians(1) - # print("left_radians: %s" % left_radians) - # if not left_radians: - # left_radians = [-0.008, 0.073, -0.008, 0.162, -0.479, 0.767] - # print("set left_radians: %s" % left_radians) - # elif left_radians: - # print("left_radians: %s" % left_radians) - - # =======right_radians======= - # rencoder = mb.get_encoders(2) - # right_radians = [] - # for index, value in enumerate(rencoder): - # value = (value-2048)*2*math.pi/4096 - # right_radians.append(value) right_radians = mb.get_radians(2) - # # print("right_radians: %s" % right_radians) - # if not right_radians: - # right_radians = [-0.008, -0.073, -0.008, 0.162, -0.479, 0.767] - # print("set right_radians: %s" % right_radians) - # elif right_radians: - # print("right_radians: %s" % right_radians) - - # =======waist_radian======= - - # wencoder = mb.get_encoder(3,1) + wangles = mb.get_angles(3)[0]*(math.pi/180) - print("wangles : %s" % wangles) - # waist_radian =wangles[0]*(math.pi/180) waist_radian =[] waist_radian.append(wangles) - print("waist_radian : %s" % waist_radian) - - # for index, value in enumerate(wencoder): - # value = (value-2048)*2*math.pi/4096 - # waist_radian.append((wencoder-2048)*2*math.pi/4096) - # print("waist_radian: %s "%waist_radian) - # if not waist_radian: - # waist_radian = [0] - # print("set waist_radian:%s" % waist_radian) - + print('left:',left_radians,'right:',right_radians,'w:',waist_radian) + # =======all_radians======= all_radians = left_radians + right_radians + waist_radian data_list = [] for index, value in enumerate(all_radians): data_list.append(value) + # rospy.loginfo('{}'.format(data_list)) joint_state_send.position = data_list + print("all_radians: %s" % data_list) + pub.publish(joint_state_send) # =======left_coords======= left_coords = mb.get_coords(1) - # print("left_coords: %s" % left_coords) - # if not left_coords: - # # rospy.loginfo("error [101]: can not get left_coord values") - # left_coords = [-50.4, 63.4, 411.6, -91.23, -0.08, -90.08] - # print("set lc:",left_coords) # =======right_coords======= right_coords = mb.get_coords(2) - # print("right_coords: %s " % right_coords) - # if not right_coords: - # # rospy.loginfo("error [101]: can not get right_coords values") - # right_coords = [50.4, -63.4, 411.6, -91.23, -0.08, -90.08] - # # print("set rc:",right_coords) - # right_coords = [50.4, -63.4, 411.6, -91.23, -0.08, -90.08] - # waist_coords = mb.get_coords(3) waist_coords = mb.get_angles(3) - print("waist_coords: %s " % waist_coords[0]) - # coords = left_coords + right_coords - # print("all_coords:%s" % coords) - + # marker marker_.header.stamp = rospy.Time.now() marker_.type = marker_.SPHERE @@ -178,7 +120,7 @@ def talker(): time.sleep(0.02) - marker_.pose.position.x = waist_coords[1] / 1000 * -1 + marker_.pose.position.x = waist_coords[0] / 1000 * -1 marker_.color.a = 1.0 marker_.color.r = 0.0 diff --git a/Mybuddy/mybuddy/scripts/following_marker.py b/Mybuddy/mybuddy/scripts/following_marker.py deleted file mode 100644 index 4afe3f9..0000000 --- a/Mybuddy/mybuddy/scripts/following_marker.py +++ /dev/null @@ -1,64 +0,0 @@ -#!/usr/bin/env python2 -import time - -import rospy -from sensor_msgs.msg import JointState -from std_msgs.msg import Header -from visualization_msgs.msg import Marker -import tf - - -def talker(): - rospy.init_node("following_marker", anonymous=True) - - pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) - rate = rospy.Rate(20) - - listener = tf.TransformListener() - - marker_ = Marker() - marker_.header.frame_id = "/joint1" - marker_.ns = "basic_cube" - - print("publishing ...") - while not rospy.is_shutdown(): - now = rospy.Time.now() - rospy.Duration(0.1) - - try: - trans, rot = listener.lookupTransform("joint1", "basic_shapes", now) - except Exception as e: - print(e) - continue - - print(type(trans), trans) - print(type(rot), rot) - - # marker - marker_.header.stamp = now - marker_.type = marker_.CUBE - marker_.action = marker_.ADD - marker_.scale.x = 0.04 - marker_.scale.y = 0.04 - marker_.scale.z = 0.04 - - # marker position initial - marker_.pose.position.x = trans[0] - marker_.pose.position.y = trans[1] - marker_.pose.position.z = trans[2] - marker_.pose.orientation.x = rot[0] - marker_.pose.orientation.y = rot[1] - marker_.pose.orientation.z = rot[2] - marker_.pose.orientation.w = rot[3] - - marker_.color.a = 1.0 - marker_.color.g = 1.0 - pub_marker.publish(marker_) - - rate.sleep() - - -if __name__ == "__main__": - try: - talker() - except rospy.ROSInterruptException: - pass diff --git a/Mybuddy/mybuddy/scripts/listen_real.py b/Mybuddy/mybuddy/scripts/listen_real.py index 7d34c82..6d500a7 100644 --- a/Mybuddy/mybuddy/scripts/listen_real.py +++ b/Mybuddy/mybuddy/scripts/listen_real.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 # license removed for brevity import time import math diff --git a/Mybuddy/mybuddy/scripts/simple_gui.py b/Mybuddy/mybuddy/scripts/simple_gui.py index f1eccc6..a72227a 100644 --- a/Mybuddy/mybuddy/scripts/simple_gui.py +++ b/Mybuddy/mybuddy/scripts/simple_gui.py @@ -1,6 +1,6 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # -*- coding: utf-8 -*- -import Tkinter as tk +import tkinter as tk from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus import rospy import time diff --git a/Mybuddy/mybuddy/scripts/slider_control.py b/Mybuddy/mybuddy/scripts/slider_control.py index 9b84f1e..f1fdddc 100755 --- a/Mybuddy/mybuddy/scripts/slider_control.py +++ b/Mybuddy/mybuddy/scripts/slider_control.py @@ -1,11 +1,11 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 """[summary] This file obtains the joint angle of the manipulator in ROS, and then sends it directly to the real manipulator using `pymycobot` API. This file is [slider_control.launch] related script. Passable parameters: - port: serial prot string. Defaults is '/dev/ttyUSB0' + port: serial prot string. Defaults is '/dev/ttyACM0' baud: serial prot baudrate. Defaults is 115200. """ @@ -41,7 +41,7 @@ def callback(data): mb.send_radians(2,data_list2, 50) time.sleep(0.02) - print(data_list3[0]* (180 / math.pi)) + # print(data_list3[0]* (180 / math.pi)) print("\n") mb.send_angle(3, 1, data_list3[0]* (180 / math.pi), 10) diff --git a/Mybuddy/mybuddy/scripts/teleop_keyboard.py b/Mybuddy/mybuddy/scripts/teleop_keyboard.py index ac66bff..f24da3d 100644 --- a/Mybuddy/mybuddy/scripts/teleop_keyboard.py +++ b/Mybuddy/mybuddy/scripts/teleop_keyboard.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 from __future__ import print_function from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus import rospy