mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
add mecharm_communication of mecharm
This commit is contained in:
wangWking
parent
2a76f925df
commit
b0d086bc12
25 changed files with 1495 additions and 0 deletions
216
mecharm/mecharm_communication/CMakeLists.txt
Normal file
216
mecharm/mecharm_communication/CMakeLists.txt
Normal file
|
|
@ -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)
|
||||
25
mecharm/mecharm_communication/LICENSE
Normal file
25
mecharm/mecharm_communication/LICENSE
Normal file
|
|
@ -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.
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyAMA0" />
|
||||
<arg name="baud" default="1000000" />
|
||||
|
||||
<!-- Open communication service ，开启通讯服务-->
|
||||
<node name="mecharm_services" pkg="mecharm_communication" type="mecharm_topics_jsnn.py" output="screen">
|
||||
<param name="port" type="string" value="$(arg port)" />
|
||||
<param name="baud" type="int" value="$(arg baud)" />
|
||||
</node>
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyAMA0" />
|
||||
<arg name="baud" default="1000000" />
|
||||
|
||||
<!-- Open communication service ，开启通讯服务-->
|
||||
<node name="mecharm_services" pkg="mecharm_communication" type="mecharm_topics_seeed.py" output="screen">
|
||||
<param name="port" type="string" value="$(arg port)" />
|
||||
<param name="baud" type="int" value="$(arg baud)" />
|
||||
</node>
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyAMA0" />
|
||||
<arg name="baud" default="1000000" />
|
||||
|
||||
<!-- Open communication service ，开启通讯服务-->
|
||||
<node name="mecharm_services" pkg="mecharm_communication" type="mecharm_services.py" output="screen">
|
||||
<param name="port" type="string" value="$(arg port)" />
|
||||
<param name="baud" type="int" value="$(arg baud)" />
|
||||
</node>
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyAMA0" />
|
||||
<arg name="baud" default="1000000" />
|
||||
|
||||
<!-- Open communication service，开启通讯服务 -->
|
||||
<node name="mecharm_services" pkg="mecharm_communication" type="mecharm_topics.py" output="screen">
|
||||
<param name="port" type="string" value="$(arg port)" />
|
||||
<param name="baud" type="int" value="$(arg baud)" />
|
||||
</node>
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyAMA0" />
|
||||
<arg name="baud" default="1000000" />
|
||||
|
||||
<!-- Open communication service --><!-- 开启通讯服务 -->
|
||||
<node name="mecharm_services" pkg="mecharm_communication" type="mecharm_topics_pi.py" output="screen">
|
||||
<param name="port" type="string" value="$(arg port)" />
|
||||
<param name="baud" type="int" value="$(arg baud)" />
|
||||
</node>
|
||||
</launch>
|
||||
6
mecharm/mecharm_communication/msg/MycobotAngles.msg
Normal file
6
mecharm/mecharm_communication/msg/MycobotAngles.msg
Normal file
|
|
@ -0,0 +1,6 @@
|
|||
float32 joint_1
|
||||
float32 joint_2
|
||||
float32 joint_3
|
||||
float32 joint_4
|
||||
float32 joint_5
|
||||
float32 joint_6
|
||||
6
mecharm/mecharm_communication/msg/MycobotCoords.msg
Normal file
6
mecharm/mecharm_communication/msg/MycobotCoords.msg
Normal file
|
|
@ -0,0 +1,6 @@
|
|||
float32 x
|
||||
float32 y
|
||||
float32 z
|
||||
float32 rx
|
||||
float32 ry
|
||||
float32 rz
|
||||
|
|
@ -0,0 +1 @@
|
|||
bool Status
|
||||
3
mecharm/mecharm_communication/msg/MycobotPumpStatus.msg
Normal file
3
mecharm/mecharm_communication/msg/MycobotPumpStatus.msg
Normal file
|
|
@ -0,0 +1,3 @@
|
|||
bool Status
|
||||
int8 Pin1
|
||||
int8 Pin2
|
||||
8
mecharm/mecharm_communication/msg/MycobotSetAngles.msg
Normal file
8
mecharm/mecharm_communication/msg/MycobotSetAngles.msg
Normal file
|
|
@ -0,0 +1,8 @@
|
|||
float32 joint_1
|
||||
float32 joint_2
|
||||
float32 joint_3
|
||||
float32 joint_4
|
||||
float32 joint_5
|
||||
float32 joint_6
|
||||
|
||||
int8 speed
|
||||
9
mecharm/mecharm_communication/msg/MycobotSetCoords.msg
Normal file
9
mecharm/mecharm_communication/msg/MycobotSetCoords.msg
Normal file
|
|
@ -0,0 +1,9 @@
|
|||
float32 x
|
||||
float32 y
|
||||
float32 z
|
||||
float32 rx
|
||||
float32 ry
|
||||
float32 rz
|
||||
|
||||
int8 speed
|
||||
int8 model
|
||||
86
mecharm/mecharm_communication/package.xml
Normal file
86
mecharm/mecharm_communication/package.xml
Normal file
|
|
@ -0,0 +1,86 @@
|
|||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>mecharm_communication</name>
|
||||
<version>0.1.0</version>
|
||||
<description>The mecharm_commuication package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="lijun.zhang@elephantrobotics.com">zachary</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>BSD</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/mycobot_commuication</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>actionlib</build_depend>
|
||||
<build_depend>mycobot_description</build_depend>
|
||||
<!-- <build_depend>mypalletizer_communication</build_depend> -->
|
||||
|
||||
<!-- <build_export_depend>mypalletizer_communication</build_export_depend> -->
|
||||
<build_export_depend>mycobot_description</build_export_depend>
|
||||
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>actionlib</exec_depend>
|
||||
<exec_depend>joint_state_publisher</exec_depend>
|
||||
<exec_depend>joint_state_publisher_gui</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>xacro</exec_depend>
|
||||
<exec_depend>joy</exec_depend>
|
||||
<exec_depend>rviz</exec_depend>
|
||||
<exec_depend>controller_manager</exec_depend>
|
||||
<exec_depend>python-tk</exec_depend>
|
||||
<exec_depend>mycobot_description</exec_depend>
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
152
mecharm/mecharm_communication/scripts/mecharm_services.py
Executable file
152
mecharm/mecharm_communication/scripts/mecharm_services.py
Executable file
|
|
@ -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()
|
||||
216
mecharm/mecharm_communication/scripts/mecharm_topics.py
Executable file
216
mecharm/mecharm_communication/scripts/mecharm_topics.py
Executable file
|
|
@ -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
|
||||
224
mecharm/mecharm_communication/scripts/mecharm_topics_jsnn.py
Executable file
224
mecharm/mecharm_communication/scripts/mecharm_topics_jsnn.py
Executable file
|
|
@ -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
|
||||
218
mecharm/mecharm_communication/scripts/mecharm_topics_pi.py
Executable file
218
mecharm/mecharm_communication/scripts/mecharm_topics_pi.py
Executable file
|
|
@ -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
|
||||
215
mecharm/mecharm_communication/scripts/mecharm_topics_seeed.py
Executable file
215
mecharm/mecharm_communication/scripts/mecharm_topics_seeed.py
Executable file
|
|
@ -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
|
||||
9
mecharm/mecharm_communication/srv/GetAngles.srv
Normal file
9
mecharm/mecharm_communication/srv/GetAngles.srv
Normal file
|
|
@ -0,0 +1,9 @@
|
|||
|
||||
---
|
||||
|
||||
float32 joint_1
|
||||
float32 joint_2
|
||||
float32 joint_3
|
||||
float32 joint_4
|
||||
float32 joint_5
|
||||
float32 joint_6
|
||||
9
mecharm/mecharm_communication/srv/GetCoords.srv
Normal file
9
mecharm/mecharm_communication/srv/GetCoords.srv
Normal file
|
|
@ -0,0 +1,9 @@
|
|||
|
||||
---
|
||||
|
||||
float32 x
|
||||
float32 y
|
||||
float32 z
|
||||
float32 rx
|
||||
float32 ry
|
||||
float32 rz
|
||||
5
mecharm/mecharm_communication/srv/GripperStatus.srv
Normal file
5
mecharm/mecharm_communication/srv/GripperStatus.srv
Normal file
|
|
@ -0,0 +1,5 @@
|
|||
bool Status
|
||||
|
||||
---
|
||||
|
||||
bool Flag
|
||||
7
mecharm/mecharm_communication/srv/PumpStatus.srv
Normal file
7
mecharm/mecharm_communication/srv/PumpStatus.srv
Normal file
|
|
@ -0,0 +1,7 @@
|
|||
bool Status
|
||||
int8 Pin1
|
||||
int8 Pin2
|
||||
|
||||
---
|
||||
|
||||
bool Flag
|
||||
12
mecharm/mecharm_communication/srv/SetAngles.srv
Normal file
12
mecharm/mecharm_communication/srv/SetAngles.srv
Normal file
|
|
@ -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
|
||||
13
mecharm/mecharm_communication/srv/SetCoords.srv
Normal file
13
mecharm/mecharm_communication/srv/SetCoords.srv
Normal file
|
|
@ -0,0 +1,13 @@
|
|||
float32 x
|
||||
float32 y
|
||||
float32 z
|
||||
float32 rx
|
||||
float32 ry
|
||||
float32 rz
|
||||
|
||||
int8 speed
|
||||
int8 model
|
||||
|
||||
---
|
||||
|
||||
bool Flag
|
||||
Loading…
Add table
Reference in a new issue