Merge pull request #125 from elephantrobotics/280-ros-pump-camera

280 ros pump camera
elephantrobotics · Nov 8, 2023 · d298cfe · d298cfe
2 parents 25fdeb5 + 747aefd
commit d298cfe
Show file tree

Hide file tree

Showing 42 changed files with 3,776 additions and 79 deletions.
diff --git a/mycobot_280/mycobot_280/CMakeLists.txt b/mycobot_280/mycobot_280/CMakeLists.txt
@@ -32,6 +32,7 @@ catkin_install_python(PROGRAMS
   scripts/simple_gui.py
   scripts/follow_display_gripper.py
   scripts/slider_control_gripper.py
+  scripts/listen_real_gripper.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

diff --git a/mycobot_280/mycobot_280/config/mycobot.rviz b/mycobot_280/mycobot_280/config/mycobot.rviz
@@ -8,9 +8,10 @@ Panels:
         - /Status1
         - /RobotModel1
         - /TF1
-        - /Marker1
+        - /TF1/Frames1
+        - /TF1/Tree1
       Splitter Ratio: 0.5
-    Tree Height: 657
+    Tree Height: 609
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -246,7 +247,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.2028908729553223
+      Distance: 1.1026314496994019
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -262,17 +263,17 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.32539835572242737
+      Pitch: 0.7153984904289246
       Target Frame: <Fixed Frame>
-      Yaw: 3.0853891372680664
+      Yaw: 3.8853836059570312
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 954
+  Height: 906
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000031cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000031c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c20000031cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000031c000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004000000031c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002ecfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ec000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c2000002ecfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002ec000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000400000002ec00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:

diff --git a/mycobot_280/mycobot_280/launch/simple_gui_gripper.launch b/mycobot_280/mycobot_280/launch/simple_gui_gripper.launch
@@ -20,6 +20,10 @@
 		<arg name="port" value="$(arg port)" />	
 		<arg name="baud" value="$(arg baud)" />	
     </include>
-	<node name="real_listener" pkg="mycobot_280" type="listen_real.py" />
+	<!-- <node name="real_listener_gripper" pkg="mycobot_280" type="listen_real_gripper.py" /> -->
+	<node name="real_listener_gripper" pkg="mycobot_280" type="listen_real_gripper.py" output="screen">
+        <param name="port" value="$(arg port)" />
+        <param name="baud" value="$(arg baud)" />
+    </node>
 	<node name="simple_gui" pkg="mycobot_280" type="simple_gui.py" />
 </launch>
diff --git a/mycobot_280/mycobot_280/launch/simple_gui_pump.launch b/mycobot_280/mycobot_280/launch/simple_gui_pump.launch
@@ -0,0 +1,23 @@
+<launch>
+	<!-- Select connecting device and serial port ，选择连接设备及串口-->
+	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" />
+
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_pump.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
+	<arg name="gui" default="false" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF ,将值合并到TF-->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+
+    <include file="$(find mycobot_communication)/launch/communication_service.launch">
+		<arg name="port" value="$(arg port)" />	
+		<arg name="baud" value="$(arg baud)" />	
+    </include>
+	<node name="real_listener" pkg="mycobot_280" type="listen_real.py" />
+	<node name="simple_gui" pkg="mycobot_280" type="simple_gui.py" />
+</launch>
diff --git a/mycobot_280/mycobot_280/launch/slider_control_camera_flange.launch b/mycobot_280/mycobot_280/launch/slider_control_camera_flange.launch
@@ -0,0 +1,25 @@
+<launch>
+	<!-- <arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" /> -->
+	<!-- Load file model ，加载文件模型-->	
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_camera_flange.urdf"/>
+
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF，将值合并到TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_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)" />	 -->
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Open control script -->
+	<!-- <node name="control_slider" pkg="mycobot_280" type="slider_control.py"> 
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+	</node> -->
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>
diff --git a/mycobot_280/mycobot_280/launch/slider_control_pump.launch b/mycobot_280/mycobot_280/launch/slider_control_pump.launch
@@ -0,0 +1,24 @@
+<launch>
+	<!-- <arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" /> -->
+	<!-- Load file model ，加载文件模型-->	
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_pump.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF，将值合并到TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_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)" />	 -->
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Open control script -->
+	<!-- <node name="control_slider" pkg="mycobot_280" type="slider_control.py"> 
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+	</node> -->
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>
diff --git a/mycobot_280/mycobot_280/launch/teleop_keyboard_gripper.launch b/mycobot_280/mycobot_280/launch/teleop_keyboard_gripper.launch
@@ -21,5 +21,9 @@
 		<arg name="baud" value="$(arg baud)" />	
     </include>
 	<!-- listen and pub the real angles ，监听并发布真实角度-->
-	<node name="real_listener" pkg="mycobot_280" type="listen_real.py" />
+	<!-- <node name="real_listener_gripper" pkg="mycobot_280" type="listen_real_gripper.py" /> -->
+	<node name="real_listener_gripper" pkg="mycobot_280" type="listen_real_gripper.py" output="screen">
+        <param name="port" value="$(arg port)" />
+        <param name="baud" value="$(arg baud)" />
+    </node>
 </launch>
diff --git a/mycobot_280/mycobot_280/launch/teleop_keyboard_pump.launch b/mycobot_280/mycobot_280/launch/teleop_keyboard_pump.launch
@@ -0,0 +1,23 @@
+<launch>
+	<!-- Select connecting device and serial port ，选择连接设备及串口-->
+	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" />
+
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_pump.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
+	<arg name="gui" default="false" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF ，将值合并到TF-->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+
+    <include file="$(find mycobot_communication)/launch/communication_service.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.py" />
+</launch>
diff --git a/mycobot_280/mycobot_280/launch/test.launch b/mycobot_280/mycobot_280/launch/test.launch
@@ -1,5 +1,5 @@
 <launch>
-	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_vision.urdf"/>
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
 	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
 	<arg name="gui" default="true" />
 

diff --git a/mycobot_280/mycobot_280/launch/test_camera_flange.launch b/mycobot_280/mycobot_280/launch/test_camera_flange.launch
@@ -0,0 +1,25 @@
+<launch>
+	<!-- <arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" /> -->
+	<!-- Load file model ，加载文件模型-->	
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_camera_flange.urdf"/>
+
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF，将值合并到TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_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)" />	 -->
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Open control script -->
+	<!-- <node name="control_slider" pkg="mycobot_280" type="slider_control.py"> 
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+	</node> -->
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>
diff --git a/mycobot_280/mycobot_280/launch/test_gripper.launch b/mycobot_280/mycobot_280/launch/test_gripper.launch
@@ -0,0 +1,16 @@
+<launch>
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_gripper_parallel.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
+	<arg name="gui" default="true" />
+
+	<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="robot_state_publisher" />
+	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+		<param name="use_gui" value="$(arg gui)" />
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>
diff --git a/mycobot_280/mycobot_280/launch/test_pump.launch b/mycobot_280/mycobot_280/launch/test_pump.launch
@@ -0,0 +1,25 @@
+<launch>
+	<!-- <arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" /> -->
+	<!-- Load file model ，加载文件模型-->	
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_pump.urdf"/>
+
+	<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF，将值合并到TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_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)" />	 -->
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Open control script -->
+	<!-- <node name="control_slider" pkg="mycobot_280" type="slider_control.py"> 
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
+	</node> -->
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>
diff --git a/mycobot_280/mycobot_280/scripts/detect_marker.py b/mycobot_280/mycobot_280/scripts/detect_marker.py
diff --git a/mycobot_280/mycobot_280/scripts/follow_and_pump.py b/mycobot_280/mycobot_280/scripts/follow_and_pump.py
diff --git a/mycobot_280/mycobot_280/scripts/follow_display.py b/mycobot_280/mycobot_280/scripts/follow_display.py
diff --git a/mycobot_280/mycobot_280/scripts/follow_display_gripper.py b/mycobot_280/mycobot_280/scripts/follow_display_gripper.py
diff --git a/mycobot_280/mycobot_280/scripts/following_marker.py b/mycobot_280/mycobot_280/scripts/following_marker.py
diff --git a/mycobot_280/mycobot_280/scripts/listen_real_gripper.py b/mycobot_280/mycobot_280/scripts/listen_real_gripper.py
@@ -0,0 +1,132 @@
+#!/usr/bin/env python3
+# encoding:utf-8
+# license removed for brevity
+from distutils.log import error
+import time
+import math
+import os
+import fcntl
+
+import rospy
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+from mycobot_communication.srv import GetAngles
+from pymycobot.mycobot import MyCobot
+from rospy import ServiceException
+
+mc = None
+
+# Avoid serial port conflicts and need to be locked
+def acquire(lock_file):
+    open_mode = os.O_RDWR | os.O_CREAT | os.O_TRUNC
+    fd = os.open(lock_file, open_mode)
+
+    pid = os.getpid()
+    lock_file_fd = None
+
+    timeout = 50.0
+    start_time = current_time = time.time()
+    while current_time < start_time + timeout:
+        try:
+            # The LOCK_EX means that only one process can hold the lock
+            # The LOCK_NB means that the fcntl.flock() is not blocking
+            # and we are able to implement termination of while loop,
+            # when timeout is reached.
+            fcntl.flock(fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
+        except (IOError, OSError):
+            pass
+        else:
+            lock_file_fd = fd
+            break
+
+        # print('pid waiting for lock:%d'% pid)
+
+
+        time.sleep(1.0)
+        current_time = time.time()
+    if lock_file_fd is None:
+        os.close(fd)
+    return lock_file_fd
+
+
+def release(lock_file_fd):
+    # Do not remove the lockfile:
+    fcntl.flock(lock_file_fd, fcntl.LOCK_UN)
+    os.close(lock_file_fd)
+    return None
+
+def talker():
+    rospy.loginfo("start ...")
+
+    rospy.init_node("real_listener_gripper", anonymous=True)
+    pub = rospy.Publisher("joint_states", JointState, queue_size=10)
+    port = rospy.get_param("~port", "/dev/ttyUSB0")
+    baud = rospy.get_param("~baud", 115200)
+    mc = MyCobot(port, baud)
+    rate = rospy.Rate(30)  # 30hz
+
+    # pub joint state，发布关节状态
+    joint_state_send = JointState()
+    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",
+        "gripper_controller",
+    ]
+    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")
+
+    while True:
+        try:
+            func = rospy.ServiceProxy("get_joint_angles", GetAngles)
+            break
+        except ServiceException as e:
+            # pass
+            # print(f'error:{e}')
+            print("--------------error",e)
+
+    # 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
+        if mc:
+            lock = acquire("/tmp/mycobot_lock")
+            gripper_value = mc.get_gripper_value()
+            release(lock)
+        if gripper_value != -1:
+            gripper_value = -0.78 + round(gripper_value / 117.0, 2)
+            # print(gripper_value)            
+            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),
+            ]
+            radians_list.append(gripper_value)
+            # 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