From 547aa709441acca7c16c415eb4bc65cb1087fda9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BC=A0=E7=AB=8B=E5=86=9B?= Date: Fri, 14 May 2021 16:47:15 +0800 Subject: [PATCH] marker detect. --- CMakeLists.txt | 5 +- config/mycobot_with_marker.rviz | 216 ++++++++++++++++++++++++ launch/mycobot_detect_marker.launch | 17 ++ scripts/{camera.py => detect_marker.py} | 60 +++---- scripts/following_marker.py | 68 ++++++++ 5 files changed, 332 insertions(+), 34 deletions(-) create mode 100644 config/mycobot_with_marker.rviz create mode 100644 launch/mycobot_detect_marker.launch rename scripts/{camera.py => detect_marker.py} (70%) create mode 100755 scripts/following_marker.py diff --git a/CMakeLists.txt b/CMakeLists.txt index dccec72..0726e95 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,7 +47,8 @@ catkin_install_python(PROGRAMS scripts/teleop_keyboard.py scripts/sync_plan.py scripts/client.py - scripts/camera.py + scripts/detect_marker.py + scripts/following_marker.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) @@ -60,5 +61,7 @@ add_executable(opencv_camera src/opencv_camera) target_link_libraries(opencv_camera ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) add_executable(camera_display src/camera_display) target_link_libraries(camera_display ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) +add_executable(basic_shape src/basic_shape) +target_link_libraries(basic_shape ${catkin_LIBRARIES}) diff --git a/config/mycobot_with_marker.rviz b/config/mycobot_with_marker.rviz new file mode 100644 index 0000000..8b0dc7f --- /dev/null +++ b/config/mycobot_with_marker.rviz @@ -0,0 +1,216 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /TF1 + - /Marker1 + - /Marker1/Namespaces1 + Splitter Ratio: 0.5 + Tree Height: 775 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + joint1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint6_flange: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + basic_shapes: + Value: true + joint1: + Value: true + joint2: + Value: true + joint3: + Value: true + joint4: + Value: true + joint5: + Value: true + joint6: + Value: true + joint6_flange: + Value: true + Marker Scale: 0.300000012 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + joint1: + joint2: + joint3: + joint4: + joint5: + joint6: + joint6_flange: + basic_shapes: + {} + Update Interval: 0 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /visualization_marker + Name: Marker + Namespaces: + basic_cube: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: joint1 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.11990476 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.0706475973 + Y: -0.0814988762 + Z: 0.107583851 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.375398338 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.235389769 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1855 + X: 65 + Y: 24 diff --git a/launch/mycobot_detect_marker.launch b/launch/mycobot_detect_marker.launch new file mode 100644 index 0000000..946890a --- /dev/null +++ b/launch/mycobot_detect_marker.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/scripts/camera.py b/scripts/detect_marker.py similarity index 70% rename from scripts/camera.py rename to scripts/detect_marker.py index a1ccb4c..b61710d 100755 --- a/scripts/camera.py +++ b/scripts/detect_marker.py @@ -6,6 +6,8 @@ from geometry_msgs.msg import Twist from cv_bridge import CvBridge, CvBridgeError from sensor_msgs.msg import Image from visualization_msgs.msg import Marker +import tf +from tf.broadcaster import TransformBroadcaster import tf_conversions from mycobot_ros.srv import ( GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus) @@ -13,7 +15,7 @@ from mycobot_ros.srv import ( class image_converter: def __init__(self): - self.mark_pub = rospy.Publisher("/bebop/marker", Marker, queue_size=1) + 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() @@ -23,12 +25,12 @@ class image_converter: # rospy.wait_for_service('get_joint_coords') # rospy.wait_for_service('set_joint_coords') - try: - self.get_coords = rospy.ServiceProxy('get_joint_coords', GetCoords) - self.set_coords = rospy.ServiceProxy('set_joint_coords', SetCoords) - except: - print('Error: cannot connect service...') - exit(1) + # try: + # self.get_coords = rospy.ServiceProxy('get_joint_coords', GetCoords) + # self.set_coords = rospy.ServiceProxy('set_joint_coords', SetCoords) + # except: + # print('Error: cannot connect service...') + # exit(1) self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback) @@ -61,43 +63,35 @@ class image_converter: cv.aruco.drawDetectedMarkers(cv_image, corners) cv.aruco.drawAxis(cv_image, self.camera_matrix, self.dist_coeffs, rvec[i, :, :], tvec[i, :, :], 0.03) - res = self.get_coords() - if res.x == res.y == 0.0: - return - record_coords = [ - res.x, res.y, res.z, res.rx, res.ry, res.rz, 60, 1 - ] - print(record_coords) + xyz = tvec[0, 0, :] + + euler = rvec[0, 0, :] + tf_change = tf.transformations.quaternion_from_euler(euler[0], euler[1], euler[2]) + print('tf_change:', tf_change) + + + self.br.sendTransform(xyz, tf_change, rospy.Time.now(), 'basic_shapes', 'joint6_flange' ) + + + # res = self.get_coords() + # if res.x == res.y == 0.0: + # return + # record_coords = [ + # res.x, res.y, res.z, res.rx, res.ry, res.rz, 60, 1 + # ] + # print(record_coords) # [x, y, z, -172, 3, -46.8] cv.imshow("Image", cv_image) - - marker = Marker() - marker.header.frame_id = '/joint1' - marker.header.stamp = rospy.Time.now() - marker.type = marker.SPHERE - marker.action = marker.ADD - marker.scale.x = 0.04 - marker.scale.y = 0.04 - marker.scale.z = 0.04 - - marker.pose.position.x = 0 - marker.pose.position.y = 0 - marker.pose.position.z = 0 - - marker.color.a = 1.0 - marker.color.g = 1.0 - cv.waitKey(3) try: - self.mark_pub.publish(marker) pass except CvBridgeError as e: print e if __name__ == '__main__': try: - rospy.init_node("cv_bridge_test") + rospy.init_node("detect_marker") rospy.loginfo("Starting cv_bridge_test node") image_converter() rospy.spin() diff --git a/scripts/following_marker.py b/scripts/following_marker.py new file mode 100755 index 0000000..8f6be50 --- /dev/null +++ b/scripts/following_marker.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python2 +# license removed for brevity +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) + # now = rospy.Time.now() + # print(now) + + 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] - 0.02 + 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