mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
update code to noetic
This commit is contained in:
weijian
parent
10964f48b0
commit
02f7353b1d
12 changed files with 18 additions and 466 deletions
|
|
@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS
|
||||||
scripts/teleop_keyboard.py
|
scripts/teleop_keyboard.py
|
||||||
scripts/listen_real.py
|
scripts/listen_real.py
|
||||||
scripts/listen_real_of_topic.py
|
scripts/listen_real_of_topic.py
|
||||||
scripts/detect_marker.py
|
|
||||||
scripts/following_marker.py
|
|
||||||
scripts/follow_and_pump.py
|
|
||||||
scripts/simple_gui.py
|
scripts/simple_gui.py
|
||||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||||
)
|
)
|
||||||
|
|
|
||||||
|
|
@ -1,16 +0,0 @@
|
||||||
<launch>
|
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
|
|
||||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot_with_marker.rviz" />
|
|
||||||
<arg name="gui" default="true" />
|
|
||||||
|
|
||||||
<arg name="num" default="0" />
|
|
||||||
|
|
||||||
<include file="$(find mycobot_280)/launch/slider_control.launch">
|
|
||||||
<arg name="model" value="$(arg model)" />
|
|
||||||
<arg name="rvizconfig" value="$(arg rvizconfig)" />
|
|
||||||
<arg name="gui" value="$(arg gui)" />
|
|
||||||
</include>
|
|
||||||
<node name="opencv_camera" pkg="mycobot_280" type="opencv_camera" args="$(arg num)"/>
|
|
||||||
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
|
|
||||||
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,29 +0,0 @@
|
||||||
<launch>
|
|
||||||
<arg name="port" default="/dev/ttyUSB0" />
|
|
||||||
<arg name="baud" default="115200" />
|
|
||||||
|
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
|
|
||||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot_with_marker.rviz" />
|
|
||||||
<arg name="gui" default="false" />
|
|
||||||
|
|
||||||
<arg name="num" default="0" />
|
|
||||||
|
|
||||||
<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_communication)/launch/communication_topic.launch">
|
|
||||||
<arg name="port" value="$(arg port)" />
|
|
||||||
<arg name="baud" value="$(arg baud)" />
|
|
||||||
</include>
|
|
||||||
<!-- listen and pub the real angles -->
|
|
||||||
<node name="real_listener" pkg="mycobot_280" type="listen_real_of_topic.py" />
|
|
||||||
<!-- vision node -->
|
|
||||||
<node name="opencv_camera" pkg="mycobot_280" type="opencv_camera" args="$(arg num)"/>
|
|
||||||
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
|
|
||||||
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
|
|
||||||
</launch>
|
|
||||||
|
|
@ -6,8 +6,8 @@
|
||||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||||
|
|
||||||
<!-- Combinejoin values to TF -->
|
<!-- Combinejoin values to TF -->
|
||||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
|
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||||
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
|
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui">
|
||||||
<param name="use_gui" value="$(arg gui)" />
|
<param name="use_gui" value="$(arg gui)" />
|
||||||
<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
|
<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
|
||||||
</node>
|
</node>
|
||||||
|
|
|
||||||
|
|
@ -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()
|
|
||||||
|
|
@ -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()
|
|
||||||
|
|
@ -1,4 +1,4 @@
|
||||||
#!/usr/bin/env python2
|
#!/usr/bin/env python3
|
||||||
import time
|
import time
|
||||||
import os
|
import os
|
||||||
import rospy
|
import rospy
|
||||||
|
|
@ -41,12 +41,6 @@ def talker():
|
||||||
joint_state_send.header = Header()
|
joint_state_send.header = Header()
|
||||||
|
|
||||||
joint_state_send.name = [
|
joint_state_send.name = [
|
||||||
# "joint2_to_joint1",
|
|
||||||
# "joint3_to_joint2",
|
|
||||||
# "joint4_to_joint3",
|
|
||||||
# "joint5_to_joint4",
|
|
||||||
# "joint6_to_joint5",
|
|
||||||
# "joint6output_to_joint6",
|
|
||||||
"joint1_L",
|
"joint1_L",
|
||||||
"joint2_L",
|
"joint2_L",
|
||||||
"joint3_L",
|
"joint3_L",
|
||||||
|
|
@ -69,94 +63,42 @@ def talker():
|
||||||
marker_.header.frame_id = "/base_link1"
|
marker_.header.frame_id = "/base_link1"
|
||||||
marker_.ns = "my_namespace"
|
marker_.ns = "my_namespace"
|
||||||
|
|
||||||
print("joint_state_send:%s" % joint_state_send)
|
|
||||||
|
|
||||||
print("publishing ...")
|
print("publishing ...")
|
||||||
while not rospy.is_shutdown():
|
while not rospy.is_shutdown():
|
||||||
joint_state_send.header.stamp = rospy.Time.now()
|
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)
|
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)
|
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)
|
wangles = mb.get_angles(3)[0]*(math.pi/180)
|
||||||
print("wangles : %s" % wangles)
|
|
||||||
# waist_radian =wangles[0]*(math.pi/180)
|
|
||||||
waist_radian =[]
|
waist_radian =[]
|
||||||
waist_radian.append(wangles)
|
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=======
|
||||||
all_radians = left_radians + right_radians + waist_radian
|
all_radians = left_radians + right_radians + waist_radian
|
||||||
data_list = []
|
data_list = []
|
||||||
for index, value in enumerate(all_radians):
|
for index, value in enumerate(all_radians):
|
||||||
data_list.append(value)
|
data_list.append(value)
|
||||||
|
|
||||||
# rospy.loginfo('{}'.format(data_list))
|
# rospy.loginfo('{}'.format(data_list))
|
||||||
joint_state_send.position = data_list
|
joint_state_send.position = data_list
|
||||||
|
|
||||||
print("all_radians: %s" % data_list)
|
print("all_radians: %s" % data_list)
|
||||||
|
|
||||||
pub.publish(joint_state_send)
|
pub.publish(joint_state_send)
|
||||||
|
|
||||||
# =======left_coords=======
|
# =======left_coords=======
|
||||||
left_coords = mb.get_coords(1)
|
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=======
|
||||||
right_coords = mb.get_coords(2)
|
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)
|
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
|
||||||
marker_.header.stamp = rospy.Time.now()
|
marker_.header.stamp = rospy.Time.now()
|
||||||
marker_.type = marker_.SPHERE
|
marker_.type = marker_.SPHERE
|
||||||
|
|
@ -178,7 +120,7 @@ def talker():
|
||||||
|
|
||||||
time.sleep(0.02)
|
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.a = 1.0
|
||||||
marker_.color.r = 0.0
|
marker_.color.r = 0.0
|
||||||
|
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -1,4 +1,4 @@
|
||||||
#!/usr/bin/env python2
|
#!/usr/bin/env python3
|
||||||
# license removed for brevity
|
# license removed for brevity
|
||||||
import time
|
import time
|
||||||
import math
|
import math
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,6 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python3
|
||||||
# -*- coding: utf-8 -*-
|
# -*- coding: utf-8 -*-
|
||||||
import Tkinter as tk
|
import tkinter as tk
|
||||||
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
||||||
import rospy
|
import rospy
|
||||||
import time
|
import time
|
||||||
|
|
|
||||||
|
|
@ -1,11 +1,11 @@
|
||||||
#!/usr/bin/env python2
|
#!/usr/bin/env python3
|
||||||
|
|
||||||
"""[summary]
|
"""[summary]
|
||||||
This file obtains the joint angle of the manipulator in ROS,
|
This file obtains the joint angle of the manipulator in ROS,
|
||||||
and then sends it directly to the real manipulator using `pymycobot` API.
|
and then sends it directly to the real manipulator using `pymycobot` API.
|
||||||
This file is [slider_control.launch] related script.
|
This file is [slider_control.launch] related script.
|
||||||
Passable parameters:
|
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.
|
baud: serial prot baudrate. Defaults is 115200.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
|
|
@ -41,7 +41,7 @@ def callback(data):
|
||||||
mb.send_radians(2,data_list2, 50)
|
mb.send_radians(2,data_list2, 50)
|
||||||
time.sleep(0.02)
|
time.sleep(0.02)
|
||||||
|
|
||||||
print(data_list3[0]* (180 / math.pi))
|
# print(data_list3[0]* (180 / math.pi))
|
||||||
print("\n")
|
print("\n")
|
||||||
|
|
||||||
mb.send_angle(3, 1, data_list3[0]* (180 / math.pi), 10)
|
mb.send_angle(3, 1, data_list3[0]* (180 / math.pi), 10)
|
||||||
|
|
|
||||||
|
|
@ -1,4 +1,4 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python3
|
||||||
from __future__ import print_function
|
from __future__ import print_function
|
||||||
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
||||||
import rospy
|
import rospy
|
||||||
|
|
|
||||||
Loading…
Add table
Reference in a new issue