marker detect.

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})
 
 

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: <Fixed 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: <Fixed 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

launch/mycobot_detect_marker.launch

@@ -0,0 +1,17 @@
+<launch>
+	<arg name="model" default="$(find mycobot_ros)/urdf/mycobot_urdf.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_ros)/config/mycobot_with_marker.rviz" />
+	<arg name="gui" default="true" />
+
+	<arg name="num" default="0" />
+
+    <include file="$(find mycobot_ros)/launch/mycobot_slider.launch">
+		<arg name="model" value="$(arg model)" />	
+		<arg name="rvizconfig" value="$(arg rvizconfig)" />	
+		<arg name="gui" value="$(arg gui)" />	
+    </include>
+	<!-- <node name="real_listener" pkg="mycobot_ros" type="listen_real.py" /> -->
+	<node name="opencv_camera" pkg="mycobot_ros" type="opencv_camera" args="$(arg num)"/>
+	<node name="detect_marker" pkg="mycobot_ros" type="detect_marker.py" />
+	<node name="following_marker" pkg="mycobot_ros" type="following_marker.py" />
+</launch>

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:
+        # try:
-            self.get_coords = rospy.ServiceProxy('get_joint_coords', GetCoords)
+        #     self.get_coords = rospy.ServiceProxy('get_joint_coords', GetCoords)
-            self.set_coords = rospy.ServiceProxy('set_joint_coords', SetCoords)
+        #     self.set_coords = rospy.ServiceProxy('set_joint_coords', SetCoords)
-        except:
+        # except:
-            print('Error: cannot connect service...')
+        #     print('Error: cannot connect service...')
-            exit(1)
+        #     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()
+                xyz = tvec[0, 0, :]
-                if res.x == res.y == 0.0:
+
-                    return
+                euler = rvec[0, 0, :]
-                record_coords = [
+                tf_change = tf.transformations.quaternion_from_euler(euler[0], euler[1], euler[2])
-                    res.x, res.y, res.z, res.rx, res.ry, res.rz, 60, 1
+                print('tf_change:', tf_change)
-                ]
+
-                print(record_coords)
+
+                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()

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