mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
marker detect.
This commit is contained in:
张立军
parent
3dcb6352b1
commit
547aa70944
5 changed files with 332 additions and 34 deletions
|
|
@ -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})
|
||||
|
||||
|
||||
|
|
|
|||
216
config/mycobot_with_marker.rviz
Normal file
216
config/mycobot_with_marker.rviz
Normal file
|
|
@ -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
|
||||
17
launch/mycobot_detect_marker.launch
Normal file
17
launch/mycobot_detect_marker.launch
Normal file
|
|
@ -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>
|
||||
|
|
@ -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()
|
||||
68
scripts/following_marker.py
Executable file
68
scripts/following_marker.py
Executable file
|
|
@ -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
|
||||
Loading…
Add table
Reference in a new issue