Merge pull request #72 from elephantrobotics/mypalletizer260_pi

Mypalletizer260 pi

mycobot_communication/scripts/mycobot_services.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python2
+# -*- coding:utf-8 -*-
 import time
 import rospy
 from mycobot_communication.srv import *

mycobot_description/urdf/260_pi/mypal_260_pi.urdf

@@ -94,6 +94,7 @@
       <geometry>
        <!--- 0.0 0 -0.04 -->
        <cylinder length="0.015" radius="0.0152"/>
+        <!-- <mesh filename="package://mycobot_description/urdf/260_pi/link5.dae"/> -->
       </geometry>
       <material name = "grey">
         <color rgba = "0.5 0.5 0.5 1"/>
@@ -129,15 +130,15 @@
 
   <joint name="joint3_to_joint2" type="revolute">
     <axis xyz=" 0 0 1"/>
-    <limit effort = "1000.0" lower = "-0.213" upper = "1.106" velocity = "0"/>
+    <limit effort = "1000.0" lower = "-0.213" upper = "0.872665" velocity = "0"/>
     <parent link="link2"/>
     <child link="link3"/>
     <origin xyz= "0.0 -0.14 0   " rpy = "0 0 0 "/>  
   </joint>
 
-  <joint name="joint4_to_joint3" type="revolute">
+  <joint name="joint4_to_joint3" type="fixed">
-    <axis xyz=" 0 0 1"/>
+    <!-- <axis xyz=" 0 0 1"/> -->
-    <limit effort = "1000.0" lower = "0" upper = "1.5707963" velocity = "0"/>
+    <!-- <limit effort = "1000.0" lower = "0" upper = "1.5707963" velocity = "0"/> -->
     <parent link="link3"/>
     <child link="link4"/>
     <origin xyz= "0.134 -0.005   0.0" rpy = "3.1415926 0 0 "/>  

mypalletizer_260/mypalletizer_260/scripts/teleop_keyboard.py

@@ -138,8 +138,10 @@ def teleop_keyboard():
                     switch_gripper(False)
                 elif key == "1":
                     rsp = set_angles(*init_pose)
+                    record_coords = [res.x, res.y, res.z, res.rx, speed, model]
                 elif key in "2":
                     rsp = set_angles(*home_pose)
+                    record_coords = [res.x, res.y, res.z, res.rx, speed, model]
                 elif key in "3":
                     rep = get_angles()
                     home_pose[0] = rep.joint_1

mypalletizer_260/mypalletizer_260_pi/config/mypal_260_pi.rviz

@@ -7,6 +7,7 @@ Panels:
         - /Global Options1
         - /Status1
         - /RobotModel1
+        - /RobotModel1/Status1
         - /TF1
       Splitter Ratio: 0.5
     Tree Height: 645
@@ -170,10 +171,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.120398536
+      Pitch: 0.300398558
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.79040861
+      Yaw: 1.56040955
     Saved: ~
 Window Geometry:
   Displays:

mypalletizer_260/mypalletizer_260_pi/scripts/follow_display.py

@@ -15,8 +15,8 @@ def talker():
     rospy.init_node("display", anonymous=True)
 
     print("Try connect real mypal...")
-    port = rospy.get_param("~port", "/dev/ttyUSB0")
+    port = rospy.get_param("~port", "/dev/ttyAMA0")
-    baud = rospy.get_param("~baud", 115200)
+    baud = rospy.get_param("~baud", 1000000)
     print("port: {}, baud: {}\n".format(port, baud))
     try:
         mypa = MyPalletizer(port, baud)

mypalletizer_260/mypalletizer_260_pi/scripts/slider_control.py

@@ -26,7 +26,7 @@ def callback(data):
     data_list = []
     for index, value in enumerate(data.position):
         data_list.append(value)
-    del data_list[3] # delete the angle of joint3 to  joint4,because it do not exsist actually! 把joint3到joint4的角度删掉，因为它实际上不存在！
+    # del data_list[3] # delete the angle of joint3 to  joint4,because it do not exsist actually! 把joint3到joint4的角度删掉，因为它实际上不存在！
     # data_list[3] = data_list[4]
     # print(data_list)
     mc.send_radians(data_list, 80)
@@ -38,8 +38,8 @@ def listener():
     rospy.init_node("control_slider", anonymous=True)
 
     rospy.Subscriber("joint_states", JointState, callback)
-    port = rospy.get_param("~port", "/dev/ttyUSB0") # Select connected device. 选择连接设备
+    port = rospy.get_param("~port", "/dev/ttyAMA0") # Select connected device. 选择连接设备
-    baud = rospy.get_param("~baud", 115200)
+    baud = rospy.get_param("~baud", 1000000)
     print(port, baud)
     mc = MyPalletizer(port, baud)
 

mypalletizer_260/mypalletizer_260_pi/scripts/teleop_keyboard.py

@@ -139,8 +139,10 @@ def teleop_keyboard():
                     switch_gripper(False)
                 elif key == "1":
                     rsp = set_angles(*init_pose)
+                    record_coords = [res.x, res.y, res.z, res.rx, speed, model]
                 elif key in "2":
                     rsp = set_angles(*home_pose)
+                    record_coords = [res.x, res.y, res.z, res.rx, speed, model]
                 elif key in "3":
                     rep = get_angles()
                     home_pose[0] = rep.joint_1