diff --git a/mypalletizer_communication/CMakeLists.txt b/mypalletizer_communication/CMakeLists.txt deleted file mode 100644 index c501f28..0000000 --- a/mypalletizer_communication/CMakeLists.txt +++ /dev/null @@ -1,216 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(mypalletizer_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 - MypalAngles.msg - MypalCoords.msg - MypalSetAngles.msg - MypalSetCoords.msg - MypalGripperStatus.msg - MypalPumpStatus.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}/Mypal_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/Mypal_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/mypal_services.py - scripts/mypal_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_Mypal_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/mypalletizer_communication/LICENSE b/mypalletizer_communication/LICENSE deleted file mode 100644 index b8468e6..0000000 --- a/mypalletizer_communication/LICENSE +++ /dev/null @@ -1,25 +0,0 @@ -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/mypalletizer_communication/launch/communication_jsnn.launch b/mypalletizer_communication/launch/communication_jsnn.launch deleted file mode 100644 index 5668bab..0000000 --- a/mypalletizer_communication/launch/communication_jsnn.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/mypalletizer_communication/launch/communication_seeed.launch b/mypalletizer_communication/launch/communication_seeed.launch deleted file mode 100644 index f40df63..0000000 --- a/mypalletizer_communication/launch/communication_seeed.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/mypalletizer_communication/launch/communication_service.launch b/mypalletizer_communication/launch/communication_service.launch deleted file mode 100644 index 92f77b5..0000000 --- a/mypalletizer_communication/launch/communication_service.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/mypalletizer_communication/launch/communication_topic.launch b/mypalletizer_communication/launch/communication_topic.launch deleted file mode 100644 index 4ea191b..0000000 --- a/mypalletizer_communication/launch/communication_topic.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/mypalletizer_communication/launch/communication_topic_pi.launch b/mypalletizer_communication/launch/communication_topic_pi.launch deleted file mode 100644 index c436d28..0000000 --- a/mypalletizer_communication/launch/communication_topic_pi.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/mypalletizer_communication/msg/MypalAngles.msg b/mypalletizer_communication/msg/MypalAngles.msg deleted file mode 100644 index bba2181..0000000 --- a/mypalletizer_communication/msg/MypalAngles.msg +++ /dev/null @@ -1,7 +0,0 @@ -# float32 base -float32 joint_1 -float32 joint_2 -float32 joint_3 -float32 joint_4 -# float32 joint_5 -# float32 joint_6 diff --git a/mypalletizer_communication/msg/MypalCoords.msg b/mypalletizer_communication/msg/MypalCoords.msg deleted file mode 100644 index f2e98b9..0000000 --- a/mypalletizer_communication/msg/MypalCoords.msg +++ /dev/null @@ -1,6 +0,0 @@ -float32 x -float32 y -float32 z -float32 rx -# float32 ry -# float32 rz \ No newline at end of file diff --git a/mypalletizer_communication/msg/MypalGripperStatus.msg b/mypalletizer_communication/msg/MypalGripperStatus.msg deleted file mode 100644 index 18d6277..0000000 --- a/mypalletizer_communication/msg/MypalGripperStatus.msg +++ /dev/null @@ -1 +0,0 @@ -bool Status \ No newline at end of file diff --git a/mypalletizer_communication/msg/MypalPumpStatus.msg b/mypalletizer_communication/msg/MypalPumpStatus.msg deleted file mode 100644 index 2373241..0000000 --- a/mypalletizer_communication/msg/MypalPumpStatus.msg +++ /dev/null @@ -1,3 +0,0 @@ -bool Status -int8 Pin1 -int8 Pin2 diff --git a/mypalletizer_communication/msg/MypalSetAngles.msg b/mypalletizer_communication/msg/MypalSetAngles.msg deleted file mode 100644 index 3580a2e..0000000 --- a/mypalletizer_communication/msg/MypalSetAngles.msg +++ /dev/null @@ -1,9 +0,0 @@ -# float32 base -float32 joint_1 -float32 joint_2 -float32 joint_3 -float32 joint_4 -# float32 joint_5 -# float32 joint_6 - -int8 speed diff --git a/mypalletizer_communication/msg/MypalSetCoords.msg b/mypalletizer_communication/msg/MypalSetCoords.msg deleted file mode 100644 index 15c601f..0000000 --- a/mypalletizer_communication/msg/MypalSetCoords.msg +++ /dev/null @@ -1,9 +0,0 @@ -float32 x -float32 y -float32 z -float32 rx -# float32 ry -# float32 rz - -int8 speed -int8 model diff --git a/mypalletizer_communication/package.xml b/mypalletizer_communication/package.xml deleted file mode 100644 index a7c96fc..0000000 --- a/mypalletizer_communication/package.xml +++ /dev/null @@ -1,86 +0,0 @@ - - - mypalletizer_communication - 0.1.0 - The mypalletizer_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/mypalletizer_communication/scripts/mypal_services.py b/mypalletizer_communication/scripts/mypal_services.py deleted file mode 100755 index 4ea468d..0000000 --- a/mypalletizer_communication/scripts/mypal_services.py +++ /dev/null @@ -1,149 +0,0 @@ -#!/usr/bin/env python2 -import time -import rospy -# from mycobot_communication.srv import * -from mypalletizer_communication.srv import * - -# from pymycobot.mycobot import MyCobot -from pymycobot.mypalletizer import MyPalletizer - -mc = None - - -def create_handle(): - global mc - rospy.init_node("mypal_services") - rospy.loginfo("start ...") - port = rospy.get_param("~port") - baud = rospy.get_param("~baud") - rospy.loginfo("%s,%s" % (port, baud)) - mc = MyPalletizer(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): - 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): - 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,夹爪开关""" - 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 = """ -Mypalletizer Status --------------------------------- -Joint Limit: - joint 1: -160 ~ +160 - joint 2: -0.87 ~ +100.01 - joint 3: -17.13 ~ +60 - joint 4: simple show - joint 5: -170 ~ +170 - -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/mypalletizer_communication/scripts/mypal_topics.py b/mypalletizer_communication/scripts/mypal_topics.py deleted file mode 100755 index 0ec7dcd..0000000 --- a/mypalletizer_communication/scripts/mypal_topics.py +++ /dev/null @@ -1,216 +0,0 @@ -#!/usr/bin/env python2 -import time -import os -import sys -import signal -import threading - -import rospy -from pymycobot.mypalletizer import MyPalletizer - -from mypalletizer_communication import( - MypalAngles, - MypalCoords, - MypalSetAngles, - MypalSetCoords, - MypalGripperStatus, - MypalPumpStatus, -) - - -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 MypalTopics(object): - def __init__(self): - super(MypalTopics, self).__init__() - - rospy.init_node("Mypal_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 = MyPalletizer(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("Mypal/angles_real", - MypalAngles, queue_size=5) - ma = MypalAngles() - 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("Mypal/coords_real", - MypalCoords, queue_size=5) - ma = MypalCoords() - - 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( - "Mypal/angles_goal", MypalSetAngles, 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] - angles = [data.x, data.y, data.z, data.rx] - - sp = int(data.speed) - model = int(data.model) - self.mc.send_coords(angles, sp, model) - - sub = rospy.Subscriber( - "Mypal/coords_goal", MypalSetCoords, 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( - "Mypal/gripper_status", MypalGripperStatus, 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( - "Mypal/pump_status", MypalPumpStatus, callback=callback - ) - rospy.spin() - - -if __name__ == "__main__": - Watcher() - mc_topics = MypalTopics() - mc_topics.start() - # while True: - # mc_topics.pub_real_coords() - # mc_topics.sub_set_angles() - pass diff --git a/mypalletizer_communication/scripts/mypal_topics_seeed.py b/mypalletizer_communication/scripts/mypal_topics_seeed.py deleted file mode 100644 index c4754cf..0000000 --- a/mypalletizer_communication/scripts/mypal_topics_seeed.py +++ /dev/null @@ -1,212 +0,0 @@ -#!/usr/bin/env python2 -import time -import os -import sys -import signal -import threading - -import rospy - -from mypalletizer_communication import ( - MypalAngles, - MypalCoords, - MypalSetAngles, - MypalSetCoords, - MypalGripperStatus, - MypalPumpStatus, -) -from pymycobot.mypalletizer import MyPalletizer - - -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 MypalTopics(object): - def __init__(self): - super(MypalTopics, self).__init__() - - rospy.init_node("Mypal_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 = MyPalletizer(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("Mypal/angles_real", MypalAngles, queue_size=5) - ma = MypalAngles() - 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("Mypal/coords_real", MypalCoords, queue_size=5) - ma = MypalCoords() - - 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( - "Mypal/angles_goal", MypalSetAngles, callback=callback - ) - rospy.spin() - - def sub_set_coords(self): - def callback(data): - angles = [data.x, data.y, data.z, data.rx] - sp = int(data.speed) - model = int(data.model) - self.mc.send_coords(angles, sp, model) - - sub = rospy.Subscriber( - "Mypal/coords_goal", MypalSetCoords, 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( - "Mypal/gripper_status", MypalGripperStatus, 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( - "Mypal/pump_status", MypalPumpStatus, callback=callback - ) - rospy.spin() - -if __name__ == "__main__": - Watcher() - mc_topics = MypalTopics() - mc_topics.start() - # while True: - # mc_topics.pub_real_coords() - # mc_topics.sub_set_angles() - pass diff --git a/mypalletizer_communication/scripts/test.py b/mypalletizer_communication/scripts/test.py deleted file mode 100644 index e9cb605..0000000 --- a/mypalletizer_communication/scripts/test.py +++ /dev/null @@ -1,7 +0,0 @@ -from pymycobot.mypalletizer import MyPalletizer -mp = MyPalletizer("/dev/ttyUSB0", 115200) -n = mp.get_angles() -# while(1): -# print(mp.get_angles()) - # print(mp.get_radians()) -mp.send_angles([100,0,0,150],30) \ No newline at end of file diff --git a/mypalletizer_communication/srv/GetAngles.srv b/mypalletizer_communication/srv/GetAngles.srv deleted file mode 100644 index dcf5f2a..0000000 --- a/mypalletizer_communication/srv/GetAngles.srv +++ /dev/null @@ -1,11 +0,0 @@ - ---- -# float32 base -float32 joint_1 -float32 joint_2 -float32 joint_3 -float32 joint_4 -# float32 joint_5 -# float32 joint_6 - - diff --git a/mypalletizer_communication/srv/GetCoords.srv b/mypalletizer_communication/srv/GetCoords.srv deleted file mode 100644 index aabe929..0000000 --- a/mypalletizer_communication/srv/GetCoords.srv +++ /dev/null @@ -1,9 +0,0 @@ - ---- - -float32 x -float32 y -float32 z -float32 rx -# float32 ry -# float32 rz \ No newline at end of file diff --git a/mypalletizer_communication/srv/GripperStatus.srv b/mypalletizer_communication/srv/GripperStatus.srv deleted file mode 100644 index 3887d75..0000000 --- a/mypalletizer_communication/srv/GripperStatus.srv +++ /dev/null @@ -1,5 +0,0 @@ -bool Status - ---- - -bool Flag \ No newline at end of file diff --git a/mypalletizer_communication/srv/PumpStatus.srv b/mypalletizer_communication/srv/PumpStatus.srv deleted file mode 100644 index 8bd4b53..0000000 --- a/mypalletizer_communication/srv/PumpStatus.srv +++ /dev/null @@ -1,7 +0,0 @@ -bool Status -int8 Pin1 -int8 Pin2 - ---- - -bool Flag \ No newline at end of file diff --git a/mypalletizer_communication/srv/SetAngles.srv b/mypalletizer_communication/srv/SetAngles.srv deleted file mode 100644 index 539dcc3..0000000 --- a/mypalletizer_communication/srv/SetAngles.srv +++ /dev/null @@ -1,15 +0,0 @@ -# float32 base -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/mypalletizer_communication/srv/SetCoords.srv b/mypalletizer_communication/srv/SetCoords.srv deleted file mode 100644 index 3989373..0000000 --- a/mypalletizer_communication/srv/SetCoords.srv +++ /dev/null @@ -1,13 +0,0 @@ -float32 x -float32 y -float32 z -float32 rx -# float32 ry -# float32 rz - -int8 speed -int8 model - ---- - -bool Flag \ No newline at end of file