diff --git a/mecharm/mecharm_communication/CMakeLists.txt b/mecharm/mecharm_communication/CMakeLists.txt new file mode 100644 index 0000000..cdf01e2 --- /dev/null +++ b/mecharm/mecharm_communication/CMakeLists.txt @@ -0,0 +1,216 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mecharm_communication) +add_compile_options(-std=c++11) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + genmsg + message_generation +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +add_message_files(FILES + MycobotAngles.msg + MycobotCoords.msg + MycobotSetAngles.msg + MycobotSetCoords.msg + MycobotGripperStatus.msg + MycobotPumpStatus.msg +) + +## Generate services in the 'srv' folder +add_service_files(FILES + GetAngles.srv + SetAngles.srv + GetCoords.srv + SetCoords.srv + GripperStatus.srv + PumpStatus.srv +) + +## Generate added messages and services +generate_messages(DEPENDENCIES std_msgs) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +## Declare a catkin package +catkin_package( + CATKIN_DEPENDS message_runtime std_msgs +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include +# ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/mycobot_commuication.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/mycobot_commuication_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +catkin_install_python(PROGRAMS + scripts/mecharm_services.py + scripts/mecharm_topics.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_mycobot_commuication.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/mecharm/mecharm_communication/LICENSE b/mecharm/mecharm_communication/LICENSE new file mode 100644 index 0000000..b8468e6 --- /dev/null +++ b/mecharm/mecharm_communication/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/mecharm/mecharm_communication/launch/communication_jsnn.launch b/mecharm/mecharm_communication/launch/communication_jsnn.launch new file mode 100644 index 0000000..ad36017 --- /dev/null +++ b/mecharm/mecharm_communication/launch/communication_jsnn.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/mecharm/mecharm_communication/launch/communication_seeed.launch b/mecharm/mecharm_communication/launch/communication_seeed.launch new file mode 100644 index 0000000..812e73c --- /dev/null +++ b/mecharm/mecharm_communication/launch/communication_seeed.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/mecharm/mecharm_communication/launch/communication_service.launch b/mecharm/mecharm_communication/launch/communication_service.launch new file mode 100644 index 0000000..b3da477 --- /dev/null +++ b/mecharm/mecharm_communication/launch/communication_service.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/mecharm/mecharm_communication/launch/communication_topic.launch b/mecharm/mecharm_communication/launch/communication_topic.launch new file mode 100644 index 0000000..45226f6 --- /dev/null +++ b/mecharm/mecharm_communication/launch/communication_topic.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/mecharm/mecharm_communication/launch/communication_topic_pi.launch b/mecharm/mecharm_communication/launch/communication_topic_pi.launch new file mode 100644 index 0000000..959557e --- /dev/null +++ b/mecharm/mecharm_communication/launch/communication_topic_pi.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/mecharm/mecharm_communication/msg/MycobotAngles.msg b/mecharm/mecharm_communication/msg/MycobotAngles.msg new file mode 100644 index 0000000..9aa7d97 --- /dev/null +++ b/mecharm/mecharm_communication/msg/MycobotAngles.msg @@ -0,0 +1,6 @@ +float32 joint_1 +float32 joint_2 +float32 joint_3 +float32 joint_4 +float32 joint_5 +float32 joint_6 diff --git a/mecharm/mecharm_communication/msg/MycobotCoords.msg b/mecharm/mecharm_communication/msg/MycobotCoords.msg new file mode 100644 index 0000000..08f6dbd --- /dev/null +++ b/mecharm/mecharm_communication/msg/MycobotCoords.msg @@ -0,0 +1,6 @@ +float32 x +float32 y +float32 z +float32 rx +float32 ry +float32 rz \ No newline at end of file diff --git a/mecharm/mecharm_communication/msg/MycobotGripperStatus.msg b/mecharm/mecharm_communication/msg/MycobotGripperStatus.msg new file mode 100644 index 0000000..18d6277 --- /dev/null +++ b/mecharm/mecharm_communication/msg/MycobotGripperStatus.msg @@ -0,0 +1 @@ +bool Status \ No newline at end of file diff --git a/mecharm/mecharm_communication/msg/MycobotPumpStatus.msg b/mecharm/mecharm_communication/msg/MycobotPumpStatus.msg new file mode 100644 index 0000000..2373241 --- /dev/null +++ b/mecharm/mecharm_communication/msg/MycobotPumpStatus.msg @@ -0,0 +1,3 @@ +bool Status +int8 Pin1 +int8 Pin2 diff --git a/mecharm/mecharm_communication/msg/MycobotSetAngles.msg b/mecharm/mecharm_communication/msg/MycobotSetAngles.msg new file mode 100644 index 0000000..3888b50 --- /dev/null +++ b/mecharm/mecharm_communication/msg/MycobotSetAngles.msg @@ -0,0 +1,8 @@ +float32 joint_1 +float32 joint_2 +float32 joint_3 +float32 joint_4 +float32 joint_5 +float32 joint_6 + +int8 speed diff --git a/mecharm/mecharm_communication/msg/MycobotSetCoords.msg b/mecharm/mecharm_communication/msg/MycobotSetCoords.msg new file mode 100644 index 0000000..d4c7e54 --- /dev/null +++ b/mecharm/mecharm_communication/msg/MycobotSetCoords.msg @@ -0,0 +1,9 @@ +float32 x +float32 y +float32 z +float32 rx +float32 ry +float32 rz + +int8 speed +int8 model diff --git a/mecharm/mecharm_communication/package.xml b/mecharm/mecharm_communication/package.xml new file mode 100644 index 0000000..9b464bd --- /dev/null +++ b/mecharm/mecharm_communication/package.xml @@ -0,0 +1,86 @@ + + + mecharm_communication + 0.1.0 + The mecharm_commuication package + + + + + zachary + + + + + + BSD + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + + rospy + std_msgs + message_generation + + roscpp + rospy + actionlib + mycobot_description + + + + 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 + message_runtime + + + + + + diff --git a/mecharm/mecharm_communication/scripts/mecharm_services.py b/mecharm/mecharm_communication/scripts/mecharm_services.py new file mode 100755 index 0000000..322127c --- /dev/null +++ b/mecharm/mecharm_communication/scripts/mecharm_services.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python2 + # -*- coding: UTF-8 -*- +import time +import rospy +from mycobot_communication.srv import * + +from pymycobot.mycobot import MyCobot + +mc = None + + +def create_handle(): + global mc + rospy.init_node("mecharm_services") + rospy.loginfo("start ...") + port = rospy.get_param("~port") + baud = rospy.get_param("~baud") + rospy.loginfo("%s,%s" % (port, baud)) + mc = MyCobot(port, baud) + + +def create_services(): + rospy.Service("set_joint_angles", SetAngles, set_angles) + rospy.Service("get_joint_angles", GetAngles, get_angles) + rospy.Service("set_joint_coords", SetCoords, set_coords) + rospy.Service("get_joint_coords", GetCoords, get_coords) + rospy.Service("switch_gripper_status", GripperStatus, switch_status) + rospy.Service("switch_pump_status", PumpStatus, toggle_pump) + rospy.loginfo("ready") + rospy.spin() + + +def set_angles(req): + """set angles，设置角度""" + angles = [ + req.joint_1, + req.joint_2, + req.joint_3, + req.joint_4, + req.joint_5, + req.joint_6, + ] + sp = req.speed + + if mc: + mc.send_angles(angles, sp) + + return SetAnglesResponse(True) + + +def get_angles(req): + """get angles,获取角度""" + if mc: + angles = mc.get_angles() + return GetAnglesResponse(*angles) + + +def set_coords(req): + coords = [ + req.x, + req.y, + req.z, + req.rx, + req.ry, + req.rz, + ] + sp = req.speed + mod = req.model + + if mc: + mc.send_coords(coords, sp, mod) + + return SetCoordsResponse(True) + + +def get_coords(req): + if mc: + coords = mc.get_coords() + return GetCoordsResponse(*coords) + + +def switch_status(req): + """Gripper switch status""" + """夹爪开关状态""" + if mc: + if req.Status: + mc.set_gripper_state(0, 80) + else: + mc.set_gripper_state(1, 80) + + return GripperStatusResponse(True) + + +def toggle_pump(req): + if mc: + if req.Status: + mc.set_basic_output(req.Pin1, 0) + mc.set_basic_output(req.Pin2, 0) + else: + mc.set_basic_output(req.Pin1, 1) + mc.set_basic_output(req.Pin2, 1) + + return PumpStatusResponse(True) + + +robot_msg = """ +MyCobot Status +-------------------------------- +Joint Limit: + joint 1: -160 ~ +160 + joint 2: -90 ~ +90 + joint 3: -180 ~ +45 + joint 4: -160 ~ +160 + joint 5: -100 ~ +100 + joint 6: -180 ~ +180 + +Connect Status: %s + +Servo Infomation: %s + +Servo Temperature: %s + +Atom Version: %s +""" + + +def output_robot_message(): + connect_status = False + servo_infomation = "unknown" + servo_temperature = "unknown" + atom_version = "unknown" + + if mc: + cn = mc.is_controller_connected() + if cn == 1: + connect_status = True + time.sleep(0.1) + si = mc.is_all_servo_enable() + if si == 1: + servo_infomation = "all connected" + + print( + robot_msg % (connect_status, servo_infomation, + servo_temperature, atom_version) + ) + + +if __name__ == "__main__": + # print(MyCobot.__dict__) + create_handle() + output_robot_message() + create_services() diff --git a/mecharm/mecharm_communication/scripts/mecharm_topics.py b/mecharm/mecharm_communication/scripts/mecharm_topics.py new file mode 100755 index 0000000..799fe4b --- /dev/null +++ b/mecharm/mecharm_communication/scripts/mecharm_topics.py @@ -0,0 +1,216 @@ +#!/usr/bin/env python2 +# -*- coding:utf-8 -*- +import time +import os +import sys +import signal +import threading + +import rospy + +from mycobot_communication.msg import ( + MycobotAngles, + MycobotCoords, + MycobotSetAngles, + MycobotSetCoords, + MycobotGripperStatus, + MycobotPumpStatus, +) + +from pymycobot.mycobot import MyCobot + + +class Watcher: + """this class solves two problems with multithreaded + programs in Python, (1) a signal might be delivered + to any thread (which is just a malfeature) and (2) if + the thread that gets the signal is waiting, the signal + is ignored (which is a bug). + + The watcher is a concurrent process (not thread) that + waits for a signal and the process that contains the + threads. See Appendix A of The Little Book of Semaphores. + http://greenteapress.com/semaphores/ + + I have only tested this on Linux. I would expect it to + work on the Macintosh and not work on Windows. + """ + + def __init__(self): + """Creates a child thread, which returns. The parent + thread waits for a KeyboardInterrupt and then kills + the child thread. + """ + self.child = os.fork() + if self.child == 0: + return + else: + self.watch() + + def watch(self): + try: + os.wait() + except KeyboardInterrupt: + # I put the capital B in KeyBoardInterrupt so I can + # tell when the Watcher gets the SIGINT + print("KeyBoardInterrupt") + self.kill() + sys.exit() + + def kill(self): + try: + os.kill(self.child, signal.SIGKILL) + except OSError: + pass + + +class MycobotTopics(object): + def __init__(self): + super(MycobotTopics, self).__init__() + + rospy.init_node("mycobot_topics") + rospy.loginfo("start ...") + port = rospy.get_param("~port", "/dev/ttyUSB0") + baud = rospy.get_param("~baud", 115200) + rospy.loginfo("%s,%s" % (port, baud)) + self.mc = MyCobot(port, baud) + self.lock = threading.Lock() + + def start(self): + pa = threading.Thread(target=self.pub_real_angles) + pb = threading.Thread(target=self.pub_real_coords) + sa = threading.Thread(target=self.sub_set_angles) + sb = threading.Thread(target=self.sub_set_coords) + sg = threading.Thread(target=self.sub_gripper_status) + sp = threading.Thread(target=self.sub_pump_status) + + pa.setDaemon(True) + pa.start() + pb.setDaemon(True) + pb.start() + sa.setDaemon(True) + sa.start() + sb.setDaemon(True) + sb.start() + sg.setDaemon(True) + sg.start() + sp.setDaemon(True) + sp.start() + + pa.join() + pb.join() + sa.join() + sb.join() + sg.join() + sp.join() + + def pub_real_angles(self): + """Publish real angle""" + """发布真实角度""" + pub = rospy.Publisher("mycobot/angles_real", + MycobotAngles, queue_size=5) + ma = MycobotAngles() + while not rospy.is_shutdown(): + self.lock.acquire() + angles = self.mc.get_angles() + self.lock.release() + if angles: + ma.joint_1 = angles[0] + ma.joint_2 = angles[1] + ma.joint_3 = angles[2] + ma.joint_4 = angles[3] + ma.joint_5 = angles[4] + ma.joint_6 = angles[5] + pub.publish(ma) + time.sleep(0.25) + + def pub_real_coords(self): + """publish real coordinates""" + """发布真实坐标""" + pub = rospy.Publisher("mycobot/coords_real", + MycobotCoords, queue_size=5) + ma = MycobotCoords() + + while not rospy.is_shutdown(): + self.lock.acquire() + coords = self.mc.get_coords() + self.lock.release() + if coords: + ma.x = coords[0] + ma.y = coords[1] + ma.z = coords[2] + ma.rx = coords[3] + ma.ry = coords[4] + ma.rz = coords[5] + pub.publish(ma) + time.sleep(0.25) + + def sub_set_angles(self): + """subscription angles""" + """订阅角度""" + def callback(data): + angles = [ + data.joint_1, + data.joint_2, + data.joint_3, + data.joint_4, + data.joint_5, + data.joint_6, + ] + sp = int(data.speed) + self.mc.send_angles(angles, sp) + + sub = rospy.Subscriber( + "mycobot/angles_goal", MycobotSetAngles, callback=callback + ) + rospy.spin() + + def sub_set_coords(self): + def callback(data): + angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz] + sp = int(data.speed) + model = int(data.model) + self.mc.send_coords(angles, sp, model) + + sub = rospy.Subscriber( + "mycobot/coords_goal", MycobotSetCoords, callback=callback + ) + rospy.spin() + + def sub_gripper_status(self): + """Subscribe to Gripper Status""" + """订阅夹爪状态""" + def callback(data): + if data.Status: + self.mc.set_gripper_state(0, 80) + else: + self.mc.set_gripper_state(1, 80) + + sub = rospy.Subscriber( + "mycobot/gripper_status", MycobotGripperStatus, callback=callback + ) + rospy.spin() + + def sub_pump_status(self): + def callback(data): + if data.Status: + self.mc.set_basic_output(data.Pin1, 0) + self.mc.set_basic_output(data.Pin2, 0) + else: + self.mc.set_basic_output(data.Pin1, 1) + self.mc.set_basic_output(data.Pin2, 1) + + sub = rospy.Subscriber( + "mycobot/pump_status", MycobotPumpStatus, callback=callback + ) + rospy.spin() + + +if __name__ == "__main__": + Watcher() + mc_topics = MycobotTopics() + mc_topics.start() + # while True: + # mc_topics.pub_real_coords() + # mc_topics.sub_set_angles() + pass diff --git a/mecharm/mecharm_communication/scripts/mecharm_topics_jsnn.py b/mecharm/mecharm_communication/scripts/mecharm_topics_jsnn.py new file mode 100755 index 0000000..07304f8 --- /dev/null +++ b/mecharm/mecharm_communication/scripts/mecharm_topics_jsnn.py @@ -0,0 +1,224 @@ +#!/usr/bin/env python2 +# -*- coding:utf-8 -*- +import time +import os +import sys +import signal +import threading + +import rospy + +from mycobot_communication.msg import ( + MycobotAngles, + MycobotCoords, + MycobotSetAngles, + MycobotSetCoords, + MycobotGripperStatus, + MycobotPumpStatus, +) + + +# from pymycobot import MyCobot + +from pymycobot import MyCobotSocket # pi + + +class Watcher: + """this class solves two problems with multithreaded + programs in Python, (1) a signal might be delivered + to any thread (which is just a malfeature) and (2) if + the thread that gets the signal is waiting, the signal + is ignored (which is a bug). + + The watcher is a concurrent process (not thread) that + waits for a signal and the process that contains the + threads. See Appendix A of The Little Book of Semaphores. + http://greenteapress.com/semaphores/ + + I have only tested this on Linux. I would expect it to + work on the Macintosh and not work on Windows. + """ + + def __init__(self): + """Creates a child thread, which returns. The parent + thread waits for a KeyboardInterrupt and then kills + the child thread. + """ + self.child = os.fork() + if self.child == 0: + return + else: + self.watch() + + def watch(self): + try: + os.wait() + except KeyboardInterrupt: + # I put the capital B in KeyBoardInterrupt so I can# + # 我把大写的 B 放在 KeyBoardInterrupt 中，这样我就可以了 + # tell when the Watcher gets the SIGINT，告诉 Watcher 何时收到 SIGINT + print("KeyBoardInterrupt") + self.kill() + sys.exit() + + def kill(self): + try: + os.kill(self.child, signal.SIGKILL) + except OSError: + pass + + +class MycobotTopics(object): + def __init__(self): + super(MycobotTopics, self).__init__() + + rospy.init_node("mycobot_topics") + rospy.loginfo("start ...") + # Select connected device，选择连接设备 + port = rospy.get_param("~port", "/dev/ttyUSB0") + baud = rospy.get_param("~baud", 115200) + rospy.loginfo("%s,%s" % (port, baud)) + # self.mc = MyCobot(port,baud) + self.mc = MyCobotSocket(port, baud) # port + self.mc.connect() + + + self.lock = threading.Lock() + + def start(self): + pa = threading.Thread(target=self.pub_real_angles) + pb = threading.Thread(target=self.pub_real_coords) + sa = threading.Thread(target=self.sub_set_angles) + sb = threading.Thread(target=self.sub_set_coords) + sg = threading.Thread(target=self.sub_gripper_status) + sp = threading.Thread(target=self.sub_pump_status) + + pa.setDaemon(True) + pa.start() + pb.setDaemon(True) + pb.start() + sa.setDaemon(True) + sa.start() + sb.setDaemon(True) + sb.start() + sg.setDaemon(True) + sg.start() + sp.setDaemon(True) + sp.start() + + pa.join() + pb.join() + sa.join() + sb.join() + sg.join() + sp.join() + + def pub_real_angles(self): + """Publish real angle""" + """发布真实角度""" + pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5) + ma = MycobotAngles() + while not rospy.is_shutdown(): + self.lock.acquire() + angles = self.mc.get_angles() + self.lock.release() + if angles: + ma.joint_1 = angles[0] + ma.joint_2 = angles[1] + ma.joint_3 = angles[2] + ma.joint_4 = angles[3] + ma.joint_5 = angles[4] + ma.joint_6 = angles[5] + pub.publish(ma) + time.sleep(0.25) + + def pub_real_coords(self): + """publish real coordinates""" + """发布真实坐标""" + pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5) + ma = MycobotCoords() + + while not rospy.is_shutdown(): + self.lock.acquire() + coords = self.mc.get_coords() + self.lock.release() + if coords: + ma.x = coords[0] + ma.y = coords[1] + ma.z = coords[2] + ma.rx = coords[3] + ma.ry = coords[4] + ma.rz = coords[5] + pub.publish(ma) + time.sleep(0.25) + + def sub_set_angles(self): + """subscription angles""" + """订阅角度""" + def callback(data): + angles = [ + data.joint_1, + data.joint_2, + data.joint_3, + data.joint_4, + data.joint_5, + data.joint_6, + ] + sp = int(data.speed) + self.mc.send_angles(angles, sp) + + sub = rospy.Subscriber( + "mycobot/angles_goal", MycobotSetAngles, callback=callback + ) + rospy.spin() + + def sub_set_coords(self): + """Subscribe to coordinates""" + """订阅坐标""" + def callback(data): + angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz] + sp = int(data.speed) + model = int(data.model) + self.mc.send_coords(angles, sp, model) + + sub = rospy.Subscriber( + "mycobot/coords_goal", MycobotSetCoords, callback=callback + ) + rospy.spin() + + def sub_gripper_status(self): + """Subscribe to Gripper Status""" + """订阅夹爪状态""" + def callback(data): + if data.Status: + self.mc.set_gripper_state(0, 80) + else: + self.mc.set_gripper_state(1, 80) + + sub = rospy.Subscriber( + "mycobot/gripper_status", MycobotGripperStatus, callback=callback + ) + rospy.spin() + + def sub_pump_status(self): + def callback(data): + if data.Status: + self.mc.set_basic_output(data.Pin1, 0) + self.mc.set_basic_output(data.Pin2, 0) + else: + self.mc.set_basic_output(data.Pin1, 1) + self.mc.set_basic_output(data.Pin2, 1) + + sub = rospy.Subscriber( + "mycobot/pump_status", MycobotPumpStatus, callback=callback + ) + rospy.spin() + +if __name__ == "__main__": + Watcher() + mc_topics = MycobotTopics() + mc_topics.start() + # while True: + # mc_topics.pub_real_coords() + # mc_topics.sub_set_angles() + pass diff --git a/mecharm/mecharm_communication/scripts/mecharm_topics_pi.py b/mecharm/mecharm_communication/scripts/mecharm_topics_pi.py new file mode 100755 index 0000000..f43d0da --- /dev/null +++ b/mecharm/mecharm_communication/scripts/mecharm_topics_pi.py @@ -0,0 +1,218 @@ +#!/usr/bin/env python2 +# -*- coding:utf-8 -*- +import time +import os +import sys +import signal +import threading + +import rospy + +from mycobot_communication.msg import ( + MycobotAngles, + MycobotCoords, + MycobotSetAngles, + MycobotSetCoords, + MycobotGripperStatus, + MycobotPumpStatus, +) + + + +from pymycobot import MyCobotSocket + + +class Watcher: + """this class solves two problems with multithreaded + programs in Python, (1) a signal might be delivered + to any thread (which is just a malfeature) and (2) if + the thread that gets the signal is waiting, the signal + is ignored (which is a bug). + + The watcher is a concurrent process (not thread) that + waits for a signal and the process that contains the + threads. See Appendix A of The Little Book of Semaphores. + http://greenteapress.com/semaphores/ + + I have only tested this on Linux. I would expect it to + work on the Macintosh and not work on Windows. + """ + + def __init__(self): + """Creates a child thread, which returns. The parent + thread waits for a KeyboardInterrupt and then kills + the child thread.创建一个返回的子线程。 父线程等待 KeyboardInterrupt + 然后杀死子线程。 + """ + self.child = os.fork() + if self.child == 0: + return + else: + self.watch() + + def watch(self): + try: + os.wait() + except KeyboardInterrupt: + # I put the capital B in KeyBoardInterrupt so I can + # tell when the Watcher gets the SIGINT + print("KeyBoardInterrupt") + self.kill() + sys.exit() + + def kill(self): + try: + os.kill(self.child, signal.SIGKILL) + except OSError: + pass + + +class MycobotTopics(object): + def __init__(self): + super(MycobotTopics, self).__init__() + + rospy.init_node("mycobot_topics_pi") + rospy.loginfo("start ...") + # problem + port = rospy.get_param("~port", "/dev/ttyUSB0") + baud = rospy.get_param("~baud", 115200) + rospy.loginfo("%s,%s" % (port, baud)) + self.mc = MyCobotSocket(port, baud) # port + self.mc.connect() #pi + self.lock = threading.Lock() + + def start(self): + pa = threading.Thread(target=self.pub_real_angles) + pb = threading.Thread(target=self.pub_real_coords) + sa = threading.Thread(target=self.sub_set_angles) + sb = threading.Thread(target=self.sub_set_coords) + sg = threading.Thread(target=self.sub_gripper_status) + sp = threading.Thread(target=self.sub_pump_status) + + pa.setDaemon(True) + pa.start() + pb.setDaemon(True) + pb.start() + sa.setDaemon(True) + sa.start() + sb.setDaemon(True) + sb.start() + sg.setDaemon(True) + sg.start() + sp.setDaemon(True) + sp.start() + + pa.join() + pb.join() + sa.join() + sb.join() + sg.join() + sp.join() + + def pub_real_angles(self): + """Publish real angle""" + """发布真实角度""" + pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5) + ma = MycobotAngles() + while not rospy.is_shutdown(): + self.lock.acquire() + angles = self.mc.get_angles() + self.lock.release() + if angles: + ma.joint_1 = angles[0] + ma.joint_2 = angles[1] + ma.joint_3 = angles[2] + ma.joint_4 = angles[3] + ma.joint_5 = angles[4] + ma.joint_6 = angles[5] + pub.publish(ma) + time.sleep(0.25) + + def pub_real_coords(self): + """publish real coordinates""" + """发布真实坐标""" + pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5) + ma = MycobotCoords() + + while not rospy.is_shutdown(): + self.lock.acquire() + coords = self.mc.get_coords() + self.lock.release() + if coords: + ma.x = coords[0] + ma.y = coords[1] + ma.z = coords[2] + ma.rx = coords[3] + ma.ry = coords[4] + ma.rz = coords[5] + pub.publish(ma) + time.sleep(0.25) + + def sub_set_angles(self): + """subscription angles""" + """订阅角度""" + def callback(data): + angles = [ + data.joint_1, + data.joint_2, + data.joint_3, + data.joint_4, + data.joint_5, + data.joint_6, + ] + sp = int(data.speed) + self.mc.send_angles(angles, sp) + + sub = rospy.Subscriber( + "mycobot/angles_goal", MycobotSetAngles, callback=callback + ) + rospy.spin() + + def sub_set_coords(self): + def callback(data): + angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz] + sp = int(data.speed) + model = int(data.model) + self.mc.send_coords(angles, sp, model) + + sub = rospy.Subscriber( + "mycobot/coords_goal", MycobotSetCoords, callback=callback + ) + rospy.spin() + + def sub_gripper_status(self): + """Subscribe to Gripper Status""" + """订阅夹爪状态""" + def callback(data): + if data.Status: + self.mc.set_gripper_state(0, 80) + else: + self.mc.set_gripper_state(1, 80) + + sub = rospy.Subscriber( + "mycobot/gripper_status", MycobotGripperStatus, callback=callback + ) + rospy.spin() + + def sub_pump_status(self): + def callback(data): + if data.Status: + self.mc.set_basic_output(data.Pin1, 0) + self.mc.set_basic_output(data.Pin2, 0) + else: + self.mc.set_basic_output(data.Pin1, 1) + self.mc.set_basic_output(data.Pin2, 1) + + sub = rospy.Subscriber( + "mycobot/pump_status", MycobotPumpStatus, callback=callback + ) + rospy.spin() + +if __name__ == "__main__": + Watcher() + mc_topics = MycobotTopics() + mc_topics.start() + # while True: + # mc_topics.pub_real_coords() + # mc_topics.sub_set_angles() + pass diff --git a/mecharm/mecharm_communication/scripts/mecharm_topics_seeed.py b/mecharm/mecharm_communication/scripts/mecharm_topics_seeed.py new file mode 100755 index 0000000..d1e8154 --- /dev/null +++ b/mecharm/mecharm_communication/scripts/mecharm_topics_seeed.py @@ -0,0 +1,215 @@ +#!/usr/bin/env python2 +# -*- coding:utf-8 -*- +import time +import os +import sys +import signal +import threading + +import rospy + +from mycobot_communication.msg import ( + MycobotAngles, + MycobotCoords, + MycobotSetAngles, + MycobotSetCoords, + MycobotGripperStatus, + MycobotPumpStatus, +) + + +from pymycobot import MyCobot + + +class Watcher: + """this class solves two problems with multithreaded + programs in Python, (1) a signal might be delivered + to any thread (which is just a malfeature) and (2) if + the thread that gets the signal is waiting, the signal + is ignored (which is a bug). + + The watcher is a concurrent process (not thread) that + waits for a signal and the process that contains the + threads. See Appendix A of The Little Book of Semaphores. + http://greenteapress.com/semaphores/ + + I have only tested this on Linux. I would expect it to + work on the Macintosh and not work on Windows. + """ + + def __init__(self): + """Creates a child thread, which returns. The parent + thread waits for a KeyboardInterrupt and then kills + the child thread. + """ + self.child = os.fork() + if self.child == 0: + return + else: + self.watch() + + def watch(self): + try: + os.wait() + except KeyboardInterrupt: + # I put the capital B in KeyBoardInterrupt so I can + # tell when the Watcher gets the SIGINT + print("KeyBoardInterrupt") + self.kill() + sys.exit() + + def kill(self): + try: + os.kill(self.child, signal.SIGKILL) + except OSError: + pass + + +class MycobotTopics(object): + def __init__(self): + super(MycobotTopics, self).__init__() + + rospy.init_node("mycobot_topics") + rospy.loginfo("start ...") + # problem + port = rospy.get_param("~port", "/dev/ttyUSB0") + baud = rospy.get_param("~baud", 115200) + rospy.loginfo("%s,%s" % (port, baud)) + self.mc = MyCobot(port,baud) + self.lock = threading.Lock() + + def start(self): + pa = threading.Thread(target=self.pub_real_angles) + pb = threading.Thread(target=self.pub_real_coords) + sa = threading.Thread(target=self.sub_set_angles) + sb = threading.Thread(target=self.sub_set_coords) + sg = threading.Thread(target=self.sub_gripper_status) + sp = threading.Thread(target=self.sub_pump_status) + + pa.setDaemon(True) + pa.start() + pb.setDaemon(True) + pb.start() + sa.setDaemon(True) + sa.start() + sb.setDaemon(True) + sb.start() + sg.setDaemon(True) + sg.start() + sp.setDaemon(True) + sp.start() + + pa.join() + pb.join() + sa.join() + sb.join() + sg.join() + sp.join() + + def pub_real_angles(self): + """Publish real angle""" + """发布真实角度""" + pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5) + ma = MycobotAngles() + while not rospy.is_shutdown(): + self.lock.acquire() + angles = self.mc.get_angles() + self.lock.release() + if angles: + ma.joint_1 = angles[0] + ma.joint_2 = angles[1] + ma.joint_3 = angles[2] + ma.joint_4 = angles[3] + ma.joint_5 = angles[4] + ma.joint_6 = angles[5] + pub.publish(ma) + time.sleep(0.25) + + def pub_real_coords(self): + """publish real coordinates""" + """发布真实坐标""" + pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5) + ma = MycobotCoords() + + while not rospy.is_shutdown(): + self.lock.acquire() + coords = self.mc.get_coords() + self.lock.release() + if coords: + ma.x = coords[0] + ma.y = coords[1] + ma.z = coords[2] + ma.rx = coords[3] + ma.ry = coords[4] + ma.rz = coords[5] + pub.publish(ma) + time.sleep(0.25) + + def sub_set_angles(self): + """subscription angles""" + """订阅角度""" + def callback(data): + angles = [ + data.joint_1, + data.joint_2, + data.joint_3, + data.joint_4, + data.joint_5, + data.joint_6, + ] + sp = int(data.speed) + self.mc.send_angles(angles, sp) + + sub = rospy.Subscriber( + "mycobot/angles_goal", MycobotSetAngles, callback=callback + ) + rospy.spin() + + def sub_set_coords(self): + def callback(data): + angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz] + sp = int(data.speed) + model = int(data.model) + self.mc.send_coords(angles, sp, model) + + sub = rospy.Subscriber( + "mycobot/coords_goal", MycobotSetCoords, callback=callback + ) + rospy.spin() + + def sub_gripper_status(self): + """Subscribe to Gripper Status""" + """订阅夹爪状态""" + def callback(data): + if data.Status: + self.mc.set_gripper_state(0, 80) + else: + self.mc.set_gripper_state(1, 80) + + sub = rospy.Subscriber( + "mycobot/gripper_status", MycobotGripperStatus, callback=callback + ) + rospy.spin() + + def sub_pump_status(self): + def callback(data): + if data.Status: + self.mc.set_basic_output(data.Pin1, 0) + self.mc.set_basic_output(data.Pin2, 0) + else: + self.mc.set_basic_output(data.Pin1, 1) + self.mc.set_basic_output(data.Pin2, 1) + + sub = rospy.Subscriber( + "mycobot/pump_status", MycobotPumpStatus, callback=callback + ) + rospy.spin() + +if __name__ == "__main__": + Watcher() + mc_topics = MycobotTopics() + mc_topics.start() + # while True: + # mc_topics.pub_real_coords() + # mc_topics.sub_set_angles() + pass diff --git a/mecharm/mecharm_communication/srv/GetAngles.srv b/mecharm/mecharm_communication/srv/GetAngles.srv new file mode 100644 index 0000000..6992699 --- /dev/null +++ b/mecharm/mecharm_communication/srv/GetAngles.srv @@ -0,0 +1,9 @@ + +--- + +float32 joint_1 +float32 joint_2 +float32 joint_3 +float32 joint_4 +float32 joint_5 +float32 joint_6 diff --git a/mecharm/mecharm_communication/srv/GetCoords.srv b/mecharm/mecharm_communication/srv/GetCoords.srv new file mode 100644 index 0000000..c1f9bbc --- /dev/null +++ b/mecharm/mecharm_communication/srv/GetCoords.srv @@ -0,0 +1,9 @@ + +--- + +float32 x +float32 y +float32 z +float32 rx +float32 ry +float32 rz \ No newline at end of file diff --git a/mecharm/mecharm_communication/srv/GripperStatus.srv b/mecharm/mecharm_communication/srv/GripperStatus.srv new file mode 100644 index 0000000..3887d75 --- /dev/null +++ b/mecharm/mecharm_communication/srv/GripperStatus.srv @@ -0,0 +1,5 @@ +bool Status + +--- + +bool Flag \ No newline at end of file diff --git a/mecharm/mecharm_communication/srv/PumpStatus.srv b/mecharm/mecharm_communication/srv/PumpStatus.srv new file mode 100644 index 0000000..8bd4b53 --- /dev/null +++ b/mecharm/mecharm_communication/srv/PumpStatus.srv @@ -0,0 +1,7 @@ +bool Status +int8 Pin1 +int8 Pin2 + +--- + +bool Flag \ No newline at end of file diff --git a/mecharm/mecharm_communication/srv/SetAngles.srv b/mecharm/mecharm_communication/srv/SetAngles.srv new file mode 100644 index 0000000..6f8d877 --- /dev/null +++ b/mecharm/mecharm_communication/srv/SetAngles.srv @@ -0,0 +1,12 @@ +float32 joint_1 +float32 joint_2 +float32 joint_3 +float32 joint_4 +float32 joint_5 +float32 joint_6 + +int8 speed + +--- + +bool Flag \ No newline at end of file diff --git a/mecharm/mecharm_communication/srv/SetCoords.srv b/mecharm/mecharm_communication/srv/SetCoords.srv new file mode 100644 index 0000000..e5b1e52 --- /dev/null +++ b/mecharm/mecharm_communication/srv/SetCoords.srv @@ -0,0 +1,13 @@ +float32 x +float32 y +float32 z +float32 rx +float32 ry +float32 rz + +int8 speed +int8 model + +--- + +bool Flag \ No newline at end of file