diff --git a/CobotX/cobotx_b450/CMakeLists.txt b/CobotX/cobotx_b450/CMakeLists.txt new file mode 100644 index 00000000..e6746ba4 --- /dev/null +++ b/CobotX/cobotx_b450/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 2.8.3) +project(cobotx_b450) +add_compile_options(-std=c++11) + +## Find catkin and any catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + actionlib + image_transport + cv_bridge +) + +## Declare a catkin package +catkin_package( + CATKIN_DEPENDS std_msgs actionlib +) + +## Build talker and listener +include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) + +catkin_install_python(PROGRAMS + scripts/follow_display.py + scripts/slider_control.py + 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} +) + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +# find_package(OpenCV REQUIRED) +# 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}) diff --git a/CobotX/cobotx_b450/LICENSE b/CobotX/cobotx_b450/LICENSE new file mode 100755 index 00000000..b8468e62 --- /dev/null +++ b/CobotX/cobotx_b450/LICENSE @@ -0,0 +1,25 @@ +BSD 2-Clause License + +Copyright (c) 2020, Elephant Robotics +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/CobotX/cobotx_b450/config/cobotx_b450.rviz b/CobotX/cobotx_b450/config/cobotx_b450.rviz new file mode 100755 index 00000000..53f412ae --- /dev/null +++ b/CobotX/cobotx_b450/config/cobotx_b450.rviz @@ -0,0 +1,312 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /RobotModel1/Links1/link1_L1 + - /RobotModel1/Links1/link1_L1/Position1 + - /Axes1 + - /TF1 + Splitter Ratio: 0.5 + Tree Height: 584 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +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.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference 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 + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_eye: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link7_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link7_R: + 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 + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 0.20000000298023224 + Name: Axes + Radius: 0.009999999776482582 + Reference Frame: + Show Trail: false + Value: true + - Class: rviz/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: false + base: + Value: true + body: + Value: true + head: + Value: true + head_eye: + Value: true + link1_L: + Value: true + link1_R: + Value: true + link2_L: + Value: true + link2_R: + Value: true + link3_L: + Value: true + link3_R: + Value: true + link4_L: + Value: true + link4_R: + Value: true + link5_L: + Value: true + link5_R: + Value: true + link6_L: + Value: true + link6_R: + Value: true + link7_L: + Value: true + link7_R: + Value: true + Marker Alpha: 1 + Marker Scale: 0.10000000149011612 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + base: + body: + head: + head_eye: + {} + link1_L: + link2_L: + link3_L: + link4_L: + link5_L: + link6_L: + link7_L: + {} + link1_R: + link2_R: + link3_R: + link4_R: + link5_R: + link6_R: + link7_R: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: base + 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 + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - 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: 1.5918537378311157 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5247960090637207 + Target Frame: + Yaw: 6.275412082672119 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 906 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002d3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002d3000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002d3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002d3000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073800000057fc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c7000002d300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 27 diff --git a/CobotX/cobotx_b450/launch/cobotx_b450_follow.launch b/CobotX/cobotx_b450/launch/cobotx_b450_follow.launch new file mode 100755 index 00000000..caed9947 --- /dev/null +++ b/CobotX/cobotx_b450/launch/cobotx_b450_follow.launch @@ -0,0 +1,13 @@ + + + + + + + + + ["joint_states"] + + + + diff --git a/CobotX/cobotx_b450/launch/simple_gui.launch b/CobotX/cobotx_b450/launch/simple_gui.launch new file mode 100755 index 00000000..612a126b --- /dev/null +++ b/CobotX/cobotx_b450/launch/simple_gui.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/CobotX/cobotx_b450/launch/slider_control.launch b/CobotX/cobotx_b450/launch/slider_control.launch new file mode 100755 index 00000000..63f9e222 --- /dev/null +++ b/CobotX/cobotx_b450/launch/slider_control.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/CobotX/cobotx_b450/launch/teleop_keyboard.launch b/CobotX/cobotx_b450/launch/teleop_keyboard.launch new file mode 100755 index 00000000..e9149df2 --- /dev/null +++ b/CobotX/cobotx_b450/launch/teleop_keyboard.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/CobotX/cobotx_b450/launch/test.launch b/CobotX/cobotx_b450/launch/test.launch new file mode 100755 index 00000000..9191e0a1 --- /dev/null +++ b/CobotX/cobotx_b450/launch/test.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/CobotX/cobotx_b450/package.xml b/CobotX/cobotx_b450/package.xml new file mode 100644 index 00000000..481449ac --- /dev/null +++ b/CobotX/cobotx_b450/package.xml @@ -0,0 +1,47 @@ + + + cobotx_b450 + 0.3.0 + The cobotx_b450 package + + Wangweijian + Wangweijian + + BSD + + https://github.com/elephantrobotics/mycobot_ros + + catkin + + roscpp + rospy + std_msgs + actionlib + mycobot_description + cobotx_a450_communication + + cobotx_a450_communication + mycobot_description + + roscpp + rospy + std_msgs + actionlib + joint_state_publisher + joint_state_publisher_gui + robot_state_publisher + xacro + joy + rviz + controller_manager + python-tk + mycobot_description + cobotx_a450_communication + + + + + + + + diff --git a/CobotX/cobotx_b450/scripts/follow_display.py b/CobotX/cobotx_b450/scripts/follow_display.py new file mode 100755 index 00000000..1dcb618a --- /dev/null +++ b/CobotX/cobotx_b450/scripts/follow_display.py @@ -0,0 +1,163 @@ +#!/usr/bin/env python3 +import time +import math +import rospy +from sensor_msgs.msg import JointState +from std_msgs.msg import Header +from visualization_msgs.msg import Marker + +from pymycobot.cobotx import CobotX + + +def talker(): + rospy.init_node("display", anonymous=True) + + print("Try connect real CobotX...") + port1 = rospy.get_param("~port1", "/dev/ttyS0") + port2 = rospy.get_param("~port2", "/dev/ttyTHS1") + baud = rospy.get_param("~baud", 115200) + print("port1: {}, baud: {}\n".format(port1, baud)) + print("port2: {}, baud: {}\n".format(port2, baud)) + try: + # left arm + cx1 = CobotX(port1, baud) + time.sleep(0.02) + # right arm + cx2 = CobotX(port2, baud) + except Exception as e: + print(e) + print( + """\ + \rTry connect CobotX failed! + \rPlease check wether connected with CobotX. + \rPlease chckt wether the port or baud is right. + """ + ) + exit(1) + cx1.release_all_servos() + time.sleep(0.02) + cx2.release_all_servos() + time.sleep(0.1) + print("Rlease all servos over.\n") + + pub = rospy.Publisher("joint_states", JointState, queue_size=10) + pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) + rate = rospy.Rate(30) # 30hz + + # pub joint state + joint_state_send = JointState() + joint_state_send.header = Header() + + joint_state_send.name = [ + "joint1_L", + "joint2_L", + "joint3_L", + "joint4_L", + "joint5_L", + "joint6_L", + "joint7_L", + "joint1_R", + "joint2_R", + "joint3_R", + "joint4_R", + "joint5_R", + "joint6_R", + "joint7_R", + "eye", + "head", + "body", + ] + + joint_state_send.velocity = [0] + joint_state_send.effort = [] + + marker_ = Marker() + marker_.header.frame_id = "/base" + marker_.ns = "my_namespace" + + print("publishing ...") + while not rospy.is_shutdown(): + joint_state_send.header.stamp = rospy.Time.now() + + left_angles = cx1.get_angles() + right_angles = cx2.get_angles() + eye_angle = cx2.get_angle(11) + head_angle = cx2.get_angle(12) + body_angle = cx2.get_angle(13) + + print('left:', left_angles) + print('right:', right_angles) + print('eye:', eye_angle) + print('head:', head_angle) + print('body:', body_angle) + + all_angles = left_angles + right_angles + eye_angle + head_angle + body_angle + data_list = [] + for index, value in enumerate(all_angles): + radians = math.radians(value) + data_list.append(radians) + + # rospy.loginfo('{}'.format(data_list)) + joint_state_send.position = data_list + + pub.publish(joint_state_send) + + left_coords = cx1.get_coords() + + right_coords = cx2.get_coords() + + eye_coords = cx2.get_angle(11) + + head_coords = cx2.get_angle(12) + + body_coords = cx2.get_angle(13) + + + # marker + 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 position initial.标记位置初始 + # print(coords) + # if not coords: + # coords = [0, 0, 0, 0, 0, 0] + # rospy.loginfo("error [101]: can not get coord values") + + marker_.pose.position.x = left_coords[1] / 1000 * -1 + marker_.pose.position.y = left_coords[0] / 1000 + marker_.pose.position.z = left_coords[2] / 1000 + + time.sleep(0.02) + + marker_.pose.position.x = right_coords[1] / 1000 * -1 + marker_.pose.position.y = right_coords[0] / 1000 + marker_.pose.position.z = right_coords[2] / 1000 + + time.sleep(0.02) + + marker_.pose.position.x = eye_coords[0] / 1000 * -1 + + time.sleep(0.02) + + marker_.pose.position.x = head_coords[0] / 1000 * -1 + + time.sleep(0.02) + + marker_.pose.position.x = body_coords[0] / 1000 * -1 + + 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/CobotX/cobotx_b450/scripts/listen_real.py b/CobotX/cobotx_b450/scripts/listen_real.py new file mode 100755 index 00000000..abe9f38f --- /dev/null +++ b/CobotX/cobotx_b450/scripts/listen_real.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python3 +# encoding:utf-8 +# license removed for brevity +import time +import math + +import rospy +from sensor_msgs.msg import JointState +from std_msgs.msg import Header +from cobotx_a450_communication.srv import GetAngles + + +def talker(): + rospy.loginfo("start ...") + rospy.init_node("real_listener", anonymous=True) + pub = rospy.Publisher("joint_states", JointState, queue_size=10) + rate = rospy.Rate(30) # 30hz + + # pub joint state，发布关节状态 + joint_state_send = JointState() + joint_state_send.header = Header() + + joint_state_send.name = [ + "joint1_to_base", + "joint2_to_joint1", + "joint3_to_joint2", + "joint4_to_joint3", + "joint5_to_joint4", + "joint6_to_joint5", + "joint7_to_joint6", + ] + joint_state_send.velocity = [0] + joint_state_send.effort = [] + + # waiting util server `get_joint_angles` enable.等待'get_joint_angles'服务启用 + rospy.loginfo("wait service") + rospy.wait_for_service("get_joint_angles") + func = rospy.ServiceProxy("get_joint_angles", GetAngles) + + rospy.loginfo("start loop ...") + while not rospy.is_shutdown(): + # get real angles from server.从服务器获得真实的角度。 + res = func() + if res.joint_1 == res.joint_2 == res.joint_3 == 0.0: + continue + radians_list = [ + res.joint_1 * (math.pi / 180), + res.joint_2 * (math.pi / 180), + res.joint_3 * (math.pi / 180), + res.joint_4 * (math.pi / 180), + res.joint_5 * (math.pi / 180), + res.joint_6 * (math.pi / 180), + res.joint_7 * (math.pi / 180), + ] + rospy.loginfo("res: {}".format(radians_list)) + + # publish angles.发布角度 + joint_state_send.header.stamp = rospy.Time.now() + joint_state_send.position = radians_list + pub.publish(joint_state_send) + rate.sleep() + + +if __name__ == "__main__": + try: + talker() + except rospy.ROSInterruptException: + pass diff --git a/CobotX/cobotx_b450/scripts/listen_real_of_topic.py b/CobotX/cobotx_b450/scripts/listen_real_of_topic.py new file mode 100755 index 00000000..07272f31 --- /dev/null +++ b/CobotX/cobotx_b450/scripts/listen_real_of_topic.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +# encoding:utf-8 + +import math +import rospy +from sensor_msgs.msg import JointState +from std_msgs.msg import Header +from cobotx_a450_communication.msg import CobotXAngles + + +class Listener(object): + def __init__(self): + super(Listener, self).__init__() + + rospy.loginfo("start ...") + rospy.init_node("real_listener_1", anonymous=True) + # init publisher.初始化发布者 + self.pub = rospy.Publisher("joint_states", JointState, queue_size=10) + # init subscriber.初始化订阅者 + self.sub = rospy.Subscriber("myarm/angles_real", CobotXAngles, self.callback) + rospy.spin() + + def callback(self, data): + """`cobotx/angles_real` subscriber callback method. + + Args: + data (CobotXAngles): callback argument. + """ + # ini publisher object. 初始化发布者对象 + joint_state_send = JointState() + joint_state_send.header = Header() + + joint_state_send.name = [ + "joint1_to_base", + "joint2_to_joint1", + "joint3_to_joint2", + "joint4_to_joint3", + "joint5_to_joint4", + "joint6_to_joint5", + "joint7_to_joint6", + ] + joint_state_send.velocity = [0] + joint_state_send.effort = [] + joint_state_send.header.stamp = rospy.Time.now() + + # process callback data. 处理回调数据。 + 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), + data.joint_7 * (math.pi / 180), + ] + rospy.loginfo("res: {}".format(radians_list)) + + joint_state_send.position = radians_list + self.pub.publish(joint_state_send) + + +if __name__ == "__main__": + try: + Listener() + except rospy.ROSInterruptException: + pass diff --git a/CobotX/cobotx_b450/scripts/simple_gui.py b/CobotX/cobotx_b450/scripts/simple_gui.py new file mode 100755 index 00000000..ac79b514 --- /dev/null +++ b/CobotX/cobotx_b450/scripts/simple_gui.py @@ -0,0 +1,501 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +try: + import tkinter as tk +except ImportError: + import Tkinter as tk +from cobotx_a450_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus +import rospy +import time +from rospy import ServiceException + + +class Window: + def __init__(self, handle): + self.win = handle + self.win.resizable(0, 0) # fixed window size，固定窗口大小 + + self.model = 0 + self.speed = rospy.get_param("~speed", 50) + + # set default speed，设置默认速度 + self.speed_d = tk.StringVar() + self.speed_d.set(str(self.speed)) + # print(self.speed) + self.connect_ser() + + # Get the data of the robotic arm，获取机械臂数据 + self.record_coords = [0, 0, 0, 0, 0, 0, self.speed, self.model] + self.res_angles = [0, 0, 0, 0, 0, 0, self.speed, self.model] + self.get_date() + + # get screen width and height.获取屏幕宽度和高度 + self.ws = self.win.winfo_screenwidth() # width of the screen + self.hs = self.win.winfo_screenheight() # height of the screen + # calculate x and y coordinates for the Tk root window + # 计算 Tk 根窗口的 x 和 y 坐标 + x = int((self.ws / 2) - 190) + y = int((self.hs / 2) - 250) + self.win.geometry("430x450+{}+{}".format(x, y)) + # layout,布局 + self.set_layout() + # input section,输入部分 + self.need_input() + # Show part,展示部分 + self.show_init() + + # Set the joint buttons 设置joint按钮 + tk.Button(self.frmLT, text="设置", width=5, command=self.get_joint_input).grid( + row=7, column=1, sticky="w", padx=3, pady=2 + ) + + # coordination settings button,coordination 设置按钮 + tk.Button(self.frmRT, text="设置", width=5, command=self.get_coord_input).grid( + row=7, column=1, sticky="w", padx=3, pady=2 + ) + + # Gripper switch button,夹爪开关按钮 + tk.Button(self.frmLB, text="夹爪(开)", command=self.gripper_open, width=5).grid( + row=1, column=0, sticky="w", padx=3, pady=50 + ) + tk.Button(self.frmLB, text="夹爪(关)", command=self.gripper_close, width=5).grid( + row=1, column=1, sticky="w", padx=3, pady=2 + ) + + def connect_ser(self): + rospy.init_node("simple_gui", anonymous=True, disable_signals=True) + + 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_gripper_status") + try: + self.get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords) + self.set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords) + self.get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles) + self.set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles) + self.switch_gripper = rospy.ServiceProxy( + "switch_gripper_status", GripperStatus + ) + except: + print("start error ...") + exit(1) + + print("Connect service success.") + + def set_layout(self): + self.frmLT = tk.Frame(width=200, height=200) + self.frmLC = tk.Frame(width=200, height=200) + self.frmLB = tk.Frame(width=200, height=200) + self.frmRT = tk.Frame(width=200, height=200) + self.frmLT.grid(row=0, column=0, padx=1, pady=3) + self.frmLC.grid(row=1, column=0, padx=1, pady=3) + self.frmLB.grid(row=1, column=1, padx=2, pady=3) + self.frmRT.grid(row=0, column=1, padx=2, pady=3) + + def need_input(self): + # input hint,输入提示 + tk.Label(self.frmLT, text="Joint 1 ").grid(row=0) + tk.Label(self.frmLT, text="Joint 2 ").grid(row=1) # the second row,第二行 + tk.Label(self.frmLT, text="Joint 3 ").grid(row=2) + tk.Label(self.frmLT, text="Joint 4 ").grid(row=3) + tk.Label(self.frmLT, text="Joint 5 ").grid(row=4) + tk.Label(self.frmLT, text="Joint 6 ").grid(row=5) + tk.Label(self.frmLT, text="Joint 7 ").grid(row=6) + + tk.Label(self.frmRT, text=" x ").grid(row=0) + tk.Label(self.frmRT, text=" y ").grid(row=1) # the second row,第二行 + tk.Label(self.frmRT, text=" z ").grid(row=2) + tk.Label(self.frmRT, text=" rx ").grid(row=3) + tk.Label(self.frmRT, text=" ry ").grid(row=4) + tk.Label(self.frmRT, text=" rz ").grid(row=5) + + # Set the default value of the input box,设置输入框的默认值 + self.j1_default = tk.StringVar() + self.j1_default.set(self.res_angles[0]) + self.j2_default = tk.StringVar() + self.j2_default.set(self.res_angles[1]) + self.j3_default = tk.StringVar() + self.j3_default.set(self.res_angles[2]) + self.j4_default = tk.StringVar() + self.j4_default.set(self.res_angles[3]) + self.j5_default = tk.StringVar() + self.j5_default.set(self.res_angles[4]) + self.j6_default = tk.StringVar() + self.j6_default.set(self.res_angles[5]) + self.j7_default = tk.StringVar() + self.j7_default.set(self.res_angles[6]) + + + self.x_default = tk.StringVar() + self.x_default.set(self.record_coords[0]) + self.y_default = tk.StringVar() + self.y_default.set(self.record_coords[1]) + self.z_default = tk.StringVar() + self.z_default.set(self.record_coords[2]) + self.rx_default = tk.StringVar() + self.rx_default.set(self.record_coords[3]) + self.ry_default = tk.StringVar() + self.ry_default.set(self.record_coords[4]) + self.rz_default = tk.StringVar() + self.rz_default.set(self.record_coords[5]) + + # joint input box,joint 输入框 + self.J_1 = tk.Entry(self.frmLT, textvariable=self.j1_default) + self.J_1.grid(row=0, column=1, pady=3) + self.J_2 = tk.Entry(self.frmLT, textvariable=self.j2_default) + self.J_2.grid(row=1, column=1, pady=3) + self.J_3 = tk.Entry(self.frmLT, textvariable=self.j3_default) + self.J_3.grid(row=2, column=1, pady=3) + self.J_4 = tk.Entry(self.frmLT, textvariable=self.j4_default) + self.J_4.grid(row=3, column=1, pady=3) + self.J_5 = tk.Entry(self.frmLT, textvariable=self.j5_default) + self.J_5.grid(row=4, column=1, pady=3) + self.J_6 = tk.Entry(self.frmLT, textvariable=self.j6_default) + self.J_6.grid(row=5, column=1, pady=3) + self.J_7 = tk.Entry(self.frmLT, textvariable=self.j7_default) + self.J_7.grid(row=6, column=1, pady=3) + + # coord input box,coord 输入框 + self.x = tk.Entry(self.frmRT, textvariable=self.x_default) + self.x.grid(row=0, column=1, pady=3, padx=0) + self.y = tk.Entry(self.frmRT, textvariable=self.y_default) + self.y.grid(row=1, column=1, pady=3) + self.z = tk.Entry(self.frmRT, textvariable=self.z_default) + self.z.grid(row=2, column=1, pady=3) + self.rx = tk.Entry(self.frmRT, textvariable=self.rx_default) + self.rx.grid(row=3, column=1, pady=3) + self.ry = tk.Entry(self.frmRT, textvariable=self.ry_default) + self.ry.grid(row=4, column=1, pady=3) + self.rz = tk.Entry(self.frmRT, textvariable=self.rz_default) + self.rz.grid(row=5, column=1, pady=3) + + # All input boxes, used to get the input data,所有输入框，用于拿输入的数据 + self.all_j = [self.J_1, self.J_2, self.J_3, self.J_4, self.J_5, self.J_6, self.J_7] + self.all_c = [self.x, self.y, self.z, self.rx, self.ry, self.rz] + + # speed input box,速度输入框 + tk.Label( + self.frmLB, + text="speed", + ).grid(row=0, column=0) + self.get_speed = tk.Entry(self.frmLB, textvariable=self.speed_d, width=10) + self.get_speed.grid(row=0, column=1) + + def show_init(self): + # show,显示 + tk.Label(self.frmLC, text="Joint 1 ").grid(row=0) + tk.Label(self.frmLC, text="Joint 2 ").grid(row=1) # the second row,第二行 + tk.Label(self.frmLC, text="Joint 3 ").grid(row=2) + tk.Label(self.frmLC, text="Joint 4 ").grid(row=3) + tk.Label(self.frmLC, text="Joint 5 ").grid(row=4) + tk.Label(self.frmLC, text="Joint 6 ").grid(row=5) + tk.Label(self.frmLC, text="Joint 7 ").grid(row=6) + + # get数据 + + # show,展示出来 + self.cont_1 = tk.StringVar(self.frmLC) + self.cont_1.set(str(self.res_angles[0]) + "°") + self.cont_2 = tk.StringVar(self.frmLC) + self.cont_2.set(str(self.res_angles[1]) + "°") + self.cont_3 = tk.StringVar(self.frmLC) + self.cont_3.set(str(self.res_angles[2]) + "°") + self.cont_4 = tk.StringVar(self.frmLC) + self.cont_4.set(str(self.res_angles[3]) + "°") + self.cont_5 = tk.StringVar(self.frmLC) + self.cont_5.set(str(self.res_angles[4]) + "°") + self.cont_6 = tk.StringVar(self.frmLC) + self.cont_6.set(str(self.res_angles[5]) + "°") + self.cont_7 = tk.StringVar(self.frmLC) + self.cont_7.set(str(self.res_angles[6]) + "°") + self.cont_all = [ + self.cont_1, + self.cont_2, + self.cont_3, + self.cont_4, + self.cont_5, + self.cont_6, + self.cont_7, + self.speed, + self.model, + ] + + self.show_j1 = tk.Label( + self.frmLC, + textvariable=self.cont_1, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=0, column=1, padx=0, pady=5) + + self.show_j2 = tk.Label( + self.frmLC, + textvariable=self.cont_2, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=1, column=1, padx=0, pady=5) + self.show_j3 = tk.Label( + self.frmLC, + textvariable=self.cont_3, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=2, column=1, padx=0, pady=5) + self.show_j4 = tk.Label( + self.frmLC, + textvariable=self.cont_4, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=3, column=1, padx=0, pady=5) + self.show_j5 = tk.Label( + self.frmLC, + textvariable=self.cont_5, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=4, column=1, padx=0, pady=5) + self.show_j6 = tk.Label( + self.frmLC, + textvariable=self.cont_6, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=5, column=1, padx=5, pady=5) + self.show_j7 = tk.Label( + self.frmLC, + textvariable=self.cont_7, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=6, column=1, padx=5, pady=5) + + self.all_jo = [ + self.show_j1, + self.show_j2, + self.show_j3, + self.show_j4, + self.show_j5, + self.show_j6, + self.show_j7, + ] + + # show,显示 + tk.Label(self.frmLC, text=" x ").grid(row=0, column=3) + tk.Label(self.frmLC, text=" y ").grid(row=1, column=3) + tk.Label(self.frmLC, text=" z ").grid(row=2, column=3) + tk.Label(self.frmLC, text=" rx ").grid(row=3, column=3) + tk.Label(self.frmLC, text=" ry ").grid(row=4, column=3) + tk.Label(self.frmLC, text=" rz ").grid(row=5, column=3) + self.coord_x = tk.StringVar() + self.coord_x.set(str(self.record_coords[0])) + self.coord_y = tk.StringVar() + self.coord_y.set(str(self.record_coords[1])) + self.coord_z = tk.StringVar() + self.coord_z.set(str(self.record_coords[2])) + self.coord_rx = tk.StringVar() + self.coord_rx.set(str(self.record_coords[3])) + self.coord_ry = tk.StringVar() + self.coord_ry.set(str(self.record_coords[4])) + self.coord_rz = tk.StringVar() + self.coord_rz.set(str(self.record_coords[5])) + + self.coord_all = [ + self.coord_x, + self.coord_y, + self.coord_z, + self.coord_rx, + self.coord_ry, + self.coord_rz, + self.speed, + self.model, + ] + + self.show_x = tk.Label( + self.frmLC, + textvariable=self.coord_x, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=0, column=4, padx=5, pady=5) + self.show_y = tk.Label( + self.frmLC, + textvariable=self.coord_y, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=1, column=4, padx=5, pady=5) + self.show_z = tk.Label( + self.frmLC, + textvariable=self.coord_z, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=2, column=4, padx=5, pady=5) + self.show_rx = tk.Label( + self.frmLC, + textvariable=self.coord_rx, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=3, column=4, padx=5, pady=5) + self.show_ry = tk.Label( + self.frmLC, + textvariable=self.coord_ry, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=4, column=4, padx=5, pady=5) + self.show_rz = tk.Label( + self.frmLC, + textvariable=self.coord_rz, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=5, column=4, padx=5, pady=5) + + # mm， Unit show，单位展示 + self.unit = tk.StringVar() + self.unit.set("mm") + for i in range(6): + tk.Label(self.frmLC, textvariable=self.unit, font=("Arial", 9)).grid( + row=i, column=5 + ) + + def gripper_open(self): + try: + self.switch_gripper(True) + except ServiceException: + # Probably because the method has no return value, the service throws an unhandled error + # 可能由于该方法没有返回值，服务抛出无法处理的错误 + pass + + def gripper_close(self): + try: + self.switch_gripper(False) + except ServiceException: + pass + + def get_coord_input(self): + # Get the data input by coord and send it to the robotic arm + # 获取 coord 输入的数据，发送给机械臂 + c_value = [] + for i in self.all_c: + # print(type(i.get())) + c_value.append(float(i.get())) + self.speed = ( + int(float(self.get_speed.get())) if self.get_speed.get() else self.speed + ) + c_value.append(self.speed) + c_value.append(self.model) + # print(c_value) + try: + self.set_coords(*c_value) + except ServiceException: + pass + self.show_j_date(c_value[:-2], "coord") + + def get_joint_input(self): + # Get the data input by the joint and send it to the robotic arm + # 获取joint输入的数据，发送给机械臂 + j_value = [] + for i in self.all_j: + # print(type(i.get())) + j_value.append(float(i.get())) + self.speed = ( + int(float(self.get_speed.get())) if self.get_speed.get() else self.speed + ) + j_value.append(self.speed) + + try: + self.set_angles(*j_value) + except ServiceException: + pass + self.show_j_date(j_value[:-1]) + # return j_value,c_value,speed + + def get_date(self): + # Take the data of the robotic arm for display.拿机械臂的数据，用于展示 + t = time.time() + while time.time() - t < 2: + self.res = self.get_coords() + if self.res.x > 1: + break + time.sleep(0.1) + + t = time.time() + while time.time() - t < 2: + self.angles = self.get_angles() + if self.angles.joint_1 > 1: + break + time.sleep(0.1) + # print(self.angles.joint_1) + self.record_coords = [ + round(self.res.x, 2), + round(self.res.y, 2), + round(self.res.z, 2), + round(self.res.rx, 2), + round(self.res.ry, 2), + round(self.res.rz, 2), + self.speed, + self.model, + ] + self.res_angles = [ + round(self.angles.joint_1, 2), + round(self.angles.joint_2, 2), + round(self.angles.joint_3, 2), + round(self.angles.joint_4, 2), + round(self.angles.joint_5, 2), + round(self.angles.joint_6, 2), + round(self.angles.joint_7, 2), + ] + # print('coord:',self.record_coords) + # print('angles:',self.res_angles) + + # def send_input(self,dates): + def show_j_date(self, date, way=""): + # Show data,展示数据 + if way == "coord": + for i, j in zip(date, self.coord_all): + # print(i) + j.set(str(i)) + else: + for i, j in zip(date, self.cont_all): + j.set(str(i) + "°") + + def run(self): + while True: + try: + self.win.update() + time.sleep(0.001) + except tk.TclError as e: + if "application has been destroyed" in str(e): + break + else: + raise + + +def main(): + window = tk.Tk() + window.title("CobotX ros GUI") + Window(window).run() + + +if __name__ == "__main__": + main() diff --git a/CobotX/cobotx_b450/scripts/slider_control.py b/CobotX/cobotx_b450/scripts/slider_control.py new file mode 100755 index 00000000..fca48f8c --- /dev/null +++ b/CobotX/cobotx_b450/scripts/slider_control.py @@ -0,0 +1,77 @@ +#!/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/ttyAMA1' + baud: serial prot baudrate. Defaults is 115200. +""" +import math +import time +import rospy +from sensor_msgs.msg import JointState + +from pymycobot.cobotx import CobotX + +# left arm port +cx1 = None + +# right arm port +cx2 = None + + +def callback(data): + # rospy.loginfo(rospy.get_caller_id() + "%s", data.position) + rounded_data_tuple = tuple(round(value, 2) for value in data.position) + # print(rounded_data_tuple) + data_list = [] + for index, value in enumerate(data.position): + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + print('data_list:', data_list) + left_arm = data_list[:7] + right_arm = data_list[7:-3] + middle_arm = data_list[-3:] + + print('left_arm:', left_arm) + print('right_arm:', right_arm) + print('middle_arm:', middle_arm) + + cx1.send_angles(left_arm, 50) + time.sleep(0.02) + cx2.send_angles(right_arm, 50) + time.sleep(0.02) + cx2.send_angle(11, middle_arm[0], 50) + time.sleep(0.02) + cx2.send_angle(12, middle_arm[1], 50) + time.sleep(0.02) + cx2.send_angle(13, middle_arm[2], 50) + time.sleep(0.02) + # mc.send_radians(data_list, 80) + # mc.send_angles(data_list, 80) + # time.sleep(0.5) + + +def listener(): + global cx1, cx2 + rospy.init_node("control_slider", anonymous=True) + + rospy.Subscriber("joint_states", JointState, callback) + port1 = rospy.get_param("~port1", "/dev/ttyS0") + port2 = rospy.get_param("~port2", "/dev/ttyTHS1") + baud = rospy.get_param("~baud", 115200) + print(port1, baud) + print(port2, baud) + cx1 = CobotX(port1, baud) + cx2 = CobotX(port2, baud) + + # spin() simply keeps python from exiting until this node is stopped + # spin()只是阻止python退出，直到该节点停止 + print("spin ...") + rospy.spin() + + +if __name__ == "__main__": + listener() diff --git a/CobotX/cobotx_b450/scripts/teleop_keyboard.py b/CobotX/cobotx_b450/scripts/teleop_keyboard.py new file mode 100755 index 00000000..c61fd36f --- /dev/null +++ b/CobotX/cobotx_b450/scripts/teleop_keyboard.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python3 +from __future__ import print_function +from cobotx_a450_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus +import rospy +import sys +import select +import termios +import tty +import time + +import roslib + +# Terminal output prompt information. 终端输出提示信息 +msg = """\ +CobotX Teleop Keyboard Controller +--------------------------- +Movimg options(control coordinations [x,y,z,rx,ry,rz]): + w(x+) + + a(y-) s(x-) d(y+) + + z(z-) x(z+) + +u(rx+) i(ry+) o(rz+) +j(rx-) k(ry-) l(rz-) + +Gripper control: + g - open + h - close + +Other: + 1 - Go to init pose + 2 - Go to home pose + 3 - Resave home pose + q - Quit +""" + + +def vels(speed, turn): + return "currently:\tspeed: %s\tchange percent: %s " % (speed, turn) + + +class Raw(object): + def __init__(self, stream): + self.stream = stream + self.fd = self.stream.fileno() + + def __enter__(self): + self.original_stty = termios.tcgetattr(self.stream) + tty.setcbreak(self.stream) + + def __exit__(self, type, value, traceback): + termios.tcsetattr(self.stream, termios.TCSANOW, self.original_stty) + + +def teleop_keyboard(): + rospy.init_node("teleop_keyboard") + + model = 1 + speed = rospy.get_param("~speed", 50) + change_percent = rospy.get_param("~change_percent", 5) + + change_angle = 180 * change_percent / 100 + change_len = 250 * change_percent / 100 + + 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_gripper_status") + print("service ready.") + 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) + switch_gripper = rospy.ServiceProxy( + "switch_gripper_status", GripperStatus) + except: + print("start error ...") + exit(1) + + init_pose = [0, 0, 0, 0, 0, 0, 0, speed] + home_pose = [0, 0, 0, -90, 0, 90, 0, speed] + + rsp = set_angles(*home_pose) + + while True: + res = get_coords() + if res.x > 1: + break + time.sleep(0.1) + + record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model] + print('init_coords:', record_coords) + + try: + print(msg) + print(vels(speed, change_percent)) + # Keyboard keys call different motion functions. 键盘按键调用不同的运动功能 + while 1: + try: + # print("\r current coords: %s" % record_coords, end="") + with Raw(sys.stdin): + key = sys.stdin.read(1) + if key == "q": + break + elif key in ["w", "W"]: + record_coords[0] += change_len + set_coords(*record_coords) + elif key in ["s", "S"]: + record_coords[0] -= change_len + set_coords(*record_coords) + elif key in ["a", "A"]: + record_coords[1] -= change_len + set_coords(*record_coords) + elif key in ["d", "D"]: + record_coords[1] += change_len + set_coords(*record_coords) + elif key in ["z", "Z"]: + record_coords[2] -= change_len + set_coords(*record_coords) + elif key in ["x", "X"]: + record_coords[2] += change_len + set_coords(*record_coords) + elif key in ["u", "U"]: + record_coords[3] += change_angle + set_coords(*record_coords) + elif key in ["j", "J"]: + record_coords[3] -= change_angle + set_coords(*record_coords) + elif key in ["i", "I"]: + record_coords[4] += change_angle + set_coords(*record_coords) + elif key in ["k", "K"]: + record_coords[4] -= change_angle + set_coords(*record_coords) + elif key in ["o", "O"]: + record_coords[5] += change_angle + set_coords(*record_coords) + elif key in ["l", "L"]: + record_coords[5] -= change_angle + set_coords(*record_coords) + elif key in ["g", "G"]: + switch_gripper(True) + elif key in ["h", "H"]: + switch_gripper(False) + elif key == "1": + rsp = set_angles(*init_pose) + elif key in "2": + rsp = set_angles(*home_pose) + time.sleep(3) + res = get_coords() + time.sleep(0.1) + record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model] + print('home_coords:', record_coords) + elif key in "3": + rep = get_angles() + home_pose[0] = rep.joint_1 + home_pose[1] = rep.joint_2 + home_pose[2] = rep.joint_3 + home_pose[3] = rep.joint_4 + home_pose[5] = rep.joint_5 + else: + continue + + except Exception as e: + # print(e) + continue + + except Exception as e: + print(e) + + +if __name__ == "__main__": + try: + teleop_keyboard() + except rospy.ROSInterruptException: + pass diff --git a/CobotX/cobotx_b450/scripts/test.py b/CobotX/cobotx_b450/scripts/test.py new file mode 100644 index 00000000..e02bac4d --- /dev/null +++ b/CobotX/cobotx_b450/scripts/test.py @@ -0,0 +1,23 @@ + +import time + +from pymycobot.cobotx import CobotX + +# cx = CobotX('/dev/ttyAMA1', 115200) + +data_list = [90.78, -34.22, -138.46, -76.63, 65.83, 112.31, 59.23, + -128.83, -49.48, 138.13, -103.05, -79.33, 120.49, -120.74, + 7.71, -78.49, 13.33] + + +data_left = data_list[:7] +print('data_left:', data_left) + +data_right = data_list[7: -3] +print('data_right:', data_right) + +data_mid = data_list[-3:] +print('data_mid:', data_mid) + +all_data = data_left + data_right + data_mid +print(all_data) \ No newline at end of file diff --git a/CobotX/cobotx_b450/src/camera_display.cpp b/CobotX/cobotx_b450/src/camera_display.cpp new file mode 100644 index 00000000..1f3abebf --- /dev/null +++ b/CobotX/cobotx_b450/src/camera_display.cpp @@ -0,0 +1,29 @@ +#include +#include +#include +#include + +void imageCallback(const sensor_msgs::ImageConstPtr &msg) +{ + try + { + cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image); + cv::waitKey(30); + } + catch (cv_bridge::Exception &e) + { + ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str()); + } +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "image_listener"); + ros::NodeHandle nh; + cv::namedWindow("view"); + cv::startWindowThread(); + image_transport::ImageTransport it(nh); + image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback); + ros::spin(); + cv::destroyWindow("view"); +} diff --git a/CobotX/cobotx_b450/src/opencv_camera.cpp b/CobotX/cobotx_b450/src/opencv_camera.cpp new file mode 100644 index 00000000..65018142 --- /dev/null +++ b/CobotX/cobotx_b450/src/opencv_camera.cpp @@ -0,0 +1,60 @@ +#include +#include +#include +#include +#include // for converting the command line parameter to integer,用于将命令行参数转换为整数 + +int main(int argc, char **argv) +{ + // Check if video source has been passed as a parameter,检查视频源是否已作为参数传递 + if (argv[1] == NULL) + { + ROS_INFO("argv[1]=NULL\n"); + return 1; + } + + ros::init(argc, argv, "image_publisher"); // Initialize node,初始化节点 + ros::NodeHandle nh; + image_transport::ImageTransport it(nh); + image_transport::Publisher pub = it.advertise("camera/image", 1); // Publish topic，发布话题 + + ros::Rate loop_rate(200); // refresh Hz. + + // Convert the passed as command line parameter index for the video device to an integer， + // 将作为命令行参数传递的视频设备索引转换为整数 + std::istringstream video_sourceCmd(argv[1]); + int video_source; + // Check if it is indeed a number，检查它是否确实是一个数字 + if (!(video_sourceCmd >> video_source)) + { + ROS_INFO("video_sourceCmd is %d\n", video_source); + return 1; + } + + cv::VideoCapture cap(video_source); + // Check if video device can be opened with the given index，检查是否可以使用给定的索引打开视频设备 + if (!cap.isOpened()) + { + ROS_INFO("can not opencv video device\n"); + return 1; + } + cv::Mat frame; + sensor_msgs::ImagePtr msg; + + while (nh.ok()) + { + cap >> frame; + // cv::imshow("veiwer", frame); + // Check if grabbed frame is actually full with some content，检查抓取的帧是否实际上充满了一些内容 + if (!frame.empty()) + { + msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg(); + pub.publish(msg); + //cv::Wait(1); + } + ros::spinOnce(); + loop_rate.sleep(); + // if(cv::waitKey(2) >= 0) + // break; + } +}