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