mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
chore: test detect marker and pump.
This commit is contained in:
jerryjerui
parent
f65d40da2f
commit
dd9e9f950a
6 changed files with 349 additions and 51 deletions
|
|
@ -8,14 +8,21 @@
|
|||
|
||||
<arg name="num" default="0" />
|
||||
|
||||
<include file="$(find mycobot_ros)/launch/mycobot_teleop_keyboard.launch">
|
||||
<arg name="model" value="$(arg model)" />
|
||||
<arg name="rvizconfig" value="$(arg rvizconfig)" />
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF -->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
|
||||
<!-- Show in Rviz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
|
||||
<!-- mycobot-topics -->
|
||||
<include file="$(find mycobot_ros)/launch/communication_topic.launch">
|
||||
<arg name="port" value="$(arg port)" />
|
||||
<arg name="baud" value="$(arg baud)" />
|
||||
<!-- <arg name="gui" value="$(arg gui)" /> -->
|
||||
</include>
|
||||
<!-- <node name="real_listener" pkg="mycobot_ros" type="listen_real.py" /> -->
|
||||
<!-- listen and pub the real angles -->
|
||||
<node name="real_listener" pkg="mycobot_ros" type="listen_real_of_topic.py" />
|
||||
<!-- vision node -->
|
||||
<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" />
|
||||
|
|
|
|||
133
scripts/follow_and_pump copy.py
Executable file
133
scripts/follow_and_pump copy.py
Executable file
|
|
@ -0,0 +1,133 @@
|
|||
#!/usr/bin/env python2
|
||||
import rospy
|
||||
from rospy.timer import sleep
|
||||
from visualization_msgs import msg
|
||||
from visualization_msgs.msg import Marker
|
||||
import time
|
||||
from mycobot_ros.srv import (
|
||||
GetCoords, SetCoords, GetAngles, SetAngles, PumpStatus)
|
||||
|
||||
|
||||
set_coords = None
|
||||
set_angles = None
|
||||
toggle_pump = None
|
||||
|
||||
x_offset = 15
|
||||
y_offset = 30
|
||||
z_offset = 100
|
||||
|
||||
temp_x = 0
|
||||
temp_y = 0
|
||||
temp_z = 0
|
||||
follow_z = 220
|
||||
|
||||
flag = False
|
||||
|
||||
|
||||
def compare_offset(old, new):
|
||||
count = 0
|
||||
for o, n in zip(old, new):
|
||||
if abs(o - n) < 10:
|
||||
count += 1
|
||||
|
||||
if count == len(old):
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
|
||||
|
||||
def grippercallback(data):
|
||||
global flag, temp_x, temp_y,temp_z
|
||||
print(type(data))
|
||||
if flag:
|
||||
return
|
||||
|
||||
# pump lenght: 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 (temp_x == temp_y == temp_z == 0.0) or (not compare_offset((temp_x, temp_y, temp_z), (x, y, z))):
|
||||
set_coords(x, y, follow_z, -175, 0, -90, 70, 2)
|
||||
temp_x, temp_y, temp_z = x, y, z
|
||||
time.sleep(1)
|
||||
return
|
||||
|
||||
else:
|
||||
print(x, y, z)
|
||||
|
||||
# detect heigth + pump height + limit height + offset
|
||||
x += x_offset
|
||||
y += y_offset
|
||||
z = z + 88 + 25 + z_offset
|
||||
|
||||
|
||||
set_coords(x, y, z, -175, 0, -90, 70, 2)
|
||||
time.sleep(1.5)
|
||||
|
||||
for i in range(1,5):
|
||||
|
||||
set_coords(x, y, z - i * 10, -175, 0, -90, 15, 2)
|
||||
time.sleep(.3)
|
||||
|
||||
time.sleep(2)
|
||||
|
||||
|
||||
toggle_pump(1)
|
||||
# pump on
|
||||
|
||||
set_coords(x, y, z + 20, -165, 0, -90, 70, 2)
|
||||
time.sleep(1.5)
|
||||
|
||||
set_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
time.sleep(1.5)
|
||||
|
||||
put_z = 140
|
||||
set_coords(39.4, -174.7, put_z ,-177.13, -4.13, -152.59,70,2)
|
||||
time.sleep(1.5)
|
||||
|
||||
for i in range (1,5):
|
||||
set_coords(39.4, -174.7, put_z-i*20, -177.13, -4.13, -152.59,15,2)
|
||||
time.sleep(.3)
|
||||
|
||||
toggle_pump(0)
|
||||
|
||||
set_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
time.sleep(1.5)
|
||||
|
||||
# finally
|
||||
flag = True
|
||||
|
||||
def gipper_subscriber():
|
||||
global set_coords, set_angles, toggle_pump
|
||||
# rospy.wait_for_service('get_joint_angles')
|
||||
rospy.wait_for_service('set_joint_angles')
|
||||
# rospy.wait_for_service('get_joint_coords')
|
||||
rospy.wait_for_service('set_joint_coords')
|
||||
rospy.wait_for_service('switch_pump_status')
|
||||
try:
|
||||
# get_coords = rospy.ServiceProxy('get_joint_coords', GetCoords)
|
||||
set_coords = rospy.ServiceProxy('set_joint_coords', SetCoords)
|
||||
# get_angles = rospy.ServiceProxy('get_joint_angles', GetAngles)
|
||||
set_angles = rospy.ServiceProxy('set_joint_angles', SetAngles)
|
||||
toggle_pump =rospy.ServiceProxy('switch_pump_status', PumpStatus)
|
||||
except:
|
||||
print('start error ...')
|
||||
exit(1)
|
||||
|
||||
|
||||
set_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
|
||||
time.sleep(2.5)
|
||||
rospy.init_node('gipper_subscriber',anonymous=True)
|
||||
rospy.Subscriber('visualization_marker',Marker,grippercallback, queue_size=1)
|
||||
print('gripper test')
|
||||
rospy.spin()
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
gipper_subscriber()
|
||||
|
||||
148
scripts/follow_and_pump.py
Executable file
148
scripts/follow_and_pump.py
Executable file
|
|
@ -0,0 +1,148 @@
|
|||
#!/usr/bin/env python2
|
||||
import rospy
|
||||
from visualization_msgs.msg import Marker
|
||||
import time
|
||||
import random
|
||||
|
||||
from mycobot_ros.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 = 120
|
||||
|
||||
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 lenght: 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, y, 280)
|
||||
time.sleep(.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 + 25 + z_offset
|
||||
|
||||
|
||||
pub_coords(x, y, z)
|
||||
time.sleep(2.5)
|
||||
|
||||
# down
|
||||
for i in range(1,14):
|
||||
pub_coords(x, y, z - i * 5,rx=-160, sp=10)
|
||||
time.sleep(.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,7):
|
||||
pub_coords(39.4, -174.7, put_z-i*5, -177.13, -4.13, -152.59,15,2)
|
||||
time.sleep(.1)
|
||||
|
||||
pub_pump(False)
|
||||
|
||||
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(.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()
|
||||
|
||||
|
|
@ -3,6 +3,7 @@
|
|||
|
||||
|
||||
import rospy
|
||||
from rospy.timer import sleep
|
||||
from visualization_msgs import msg
|
||||
from visualization_msgs.msg import Marker
|
||||
# from pymycobot.mycobot import MyCobot
|
||||
|
|
@ -10,15 +11,18 @@ from visualization_msgs.msg import Marker
|
|||
# from pymycobot import PI_PORT, PI_BAUD # For raspberry pi version of mycobot.
|
||||
import time
|
||||
from mycobot_ros.srv import (
|
||||
GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus)
|
||||
GetCoords, SetCoords, GetAngles, SetAngles, PumpStatus)
|
||||
|
||||
|
||||
set_coords = None
|
||||
set_angles = None
|
||||
toggle_pump = None
|
||||
|
||||
x_offset = 0
|
||||
|
||||
|
||||
x_offset = 15
|
||||
y_offset = 30
|
||||
z_offset = 60
|
||||
z_offset = 100
|
||||
|
||||
flag = False
|
||||
|
||||
|
|
@ -47,56 +51,63 @@ def grippercallback(data):
|
|||
y += y_offset
|
||||
z = z + 88 + 25 + z_offset
|
||||
|
||||
try:
|
||||
set_coords(x, y, z, -175, 0, -90, 70, 2)
|
||||
time.sleep(2.5)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
set_coords(x, y, z, -175, 0, -90, 70, 2)
|
||||
time.sleep(2.5)
|
||||
|
||||
for i in range(1,5):
|
||||
|
||||
set_coords(x, y, z - i * 10, -175, 0, -90, 15, 2)
|
||||
time.sleep(.3)
|
||||
|
||||
time.sleep(2)
|
||||
|
||||
|
||||
for i in range(1,4):
|
||||
try:
|
||||
set_coords(x, y, z - i * 10, -175, 0, -90, 70, 2)
|
||||
time.sleep(.2)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
toggle_pump(1)
|
||||
# pump on
|
||||
try:
|
||||
set_coords(x, y, z + 20, -175, 0, -90, 70, 2)
|
||||
time.sleep(2.5)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# ...
|
||||
set_coords(x, y, z + 20, -165, 0, -90, 70, 2)
|
||||
time.sleep(1.5)
|
||||
|
||||
set_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
time.sleep(1.5)
|
||||
|
||||
put_z = 140
|
||||
set_coords(39.4, -174.7, put_z ,-177.13, -4.13, -152.59,70,2)
|
||||
time.sleep(1.5)
|
||||
|
||||
for i in range (1,5):
|
||||
set_coords(39.4, -174.7, put_z-i*20, -177.13, -4.13, -152.59,15,2)
|
||||
time.sleep(.3)
|
||||
|
||||
toggle_pump(0)
|
||||
|
||||
set_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
time.sleep(1.5)
|
||||
|
||||
# finally
|
||||
flag = True
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
def gipper_subscriber():
|
||||
global set_coords, set_angles
|
||||
global set_coords, set_angles, toggle_pump
|
||||
# rospy.wait_for_service('get_joint_angles')
|
||||
rospy.wait_for_service('set_joint_angles')
|
||||
# rospy.wait_for_service('get_joint_coords')
|
||||
rospy.wait_for_service('set_joint_coords')
|
||||
rospy.wait_for_service('switch_pump_status')
|
||||
try:
|
||||
# get_coords = rospy.ServiceProxy('get_joint_coords', GetCoords)
|
||||
set_coords = rospy.ServiceProxy('set_joint_coords', SetCoords)
|
||||
# get_angles = rospy.ServiceProxy('get_joint_angles', GetAngles)
|
||||
set_angles = rospy.ServiceProxy('set_joint_angles', SetAngles)
|
||||
toggle_pump =rospy.ServiceProxy('switch_pump_status', PumpStatus)
|
||||
except:
|
||||
print('start error ...')
|
||||
exit(1)
|
||||
|
||||
try:
|
||||
set_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
set_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
|
||||
time.sleep(2.5)
|
||||
rospy.init_node('gipper_subscriber',anonymous=True)
|
||||
rospy.Subscriber('visualization_marker',Marker,grippercallback, queue_size=1)
|
||||
|
|
|
|||
|
|
@ -36,21 +36,20 @@ class Listener(object):
|
|||
joint_state_send.velocity = [0]
|
||||
joint_state_send.effort = []
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
joint_state_send.header.stamp = rospy.Time.now()
|
||||
radians_list = [
|
||||
data.joint_1 * (math.pi / 180),
|
||||
data.joint_2 * (math.pi / 180),
|
||||
data.joint_3 * (math.pi / 180),
|
||||
data.joint_4 * (math.pi / 180),
|
||||
data.joint_5 * (math.pi / 180),
|
||||
data.joint_6 * (math.pi / 180),
|
||||
]
|
||||
rospy.loginfo('res: {}'.format(radians_list))
|
||||
joint_state_send.header.stamp = rospy.Time.now()
|
||||
radians_list = [
|
||||
data.joint_1 * (math.pi / 180),
|
||||
data.joint_2 * (math.pi / 180),
|
||||
data.joint_3 * (math.pi / 180),
|
||||
data.joint_4 * (math.pi / 180),
|
||||
data.joint_5 * (math.pi / 180),
|
||||
data.joint_6 * (math.pi / 180),
|
||||
]
|
||||
rospy.loginfo('res: {}'.format(radians_list))
|
||||
|
||||
joint_state_send.position = radians_list
|
||||
self.pub.publish(joint_state_send)
|
||||
# rate.sleep()
|
||||
joint_state_send.position = radians_list
|
||||
self.pub.publish(joint_state_send)
|
||||
# rate.sleep()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
|
|
|||
|
|
@ -146,7 +146,7 @@ class MycobotTopics(object):
|
|||
angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz]
|
||||
sp = int(data.speed)
|
||||
model = int(data.model)
|
||||
self.mc.send_angles(angles, sp, model)
|
||||
self.mc.send_coords(angles, sp, model)
|
||||
|
||||
sub = rospy.Subscriber('mycobot/coords_goal', MycobotSetCoords, callback=callback)
|
||||
rospy.spin()
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue