mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
update mycobot320 code
This commit is contained in:
weijian
parent
6fbf914cee
commit
954637ea98
39 changed files with 1523 additions and 15 deletions
|
|
@ -16,7 +16,7 @@
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||||
|
|
||||||
<!-- mycobot-topics -->
|
<!-- mycobot-topics -->
|
||||||
<include file="$(find mycobot_communication)/launch/communication_topic.launch">
|
<include file="$(find mycobot_320_communication)/launch/communication_topic.launch">
|
||||||
<arg name="port" value="$(arg port)" />
|
<arg name="port" value="$(arg port)" />
|
||||||
<arg name="baud" value="$(arg baud)" />
|
<arg name="baud" value="$(arg baud)" />
|
||||||
</include>
|
</include>
|
||||||
|
|
|
||||||
|
|
@ -13,7 +13,7 @@
|
||||||
<!-- Show in Rviz -->
|
<!-- Show in Rviz -->
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||||
|
|
||||||
<include file="$(find mycobot_communication)/launch/communication_service.launch">
|
<include file="$(find mycobot_320_communication)/launch/communication_service.launch">
|
||||||
<arg name="port" value="$(arg port)" />
|
<arg name="port" value="$(arg port)" />
|
||||||
<arg name="baud" value="$(arg baud)" />
|
<arg name="baud" value="$(arg baud)" />
|
||||||
</include>
|
</include>
|
||||||
|
|
|
||||||
|
|
@ -13,7 +13,7 @@
|
||||||
<!-- Show in Rviz -->
|
<!-- Show in Rviz -->
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||||
|
|
||||||
<include file="$(find mycobot_communication)/launch/communication_service.launch">
|
<include file="$(find mycobot_320_communication)/launch/communication_service.launch">
|
||||||
<arg name="port" value="$(arg port)" />
|
<arg name="port" value="$(arg port)" />
|
||||||
<arg name="baud" value="$(arg baud)" />
|
<arg name="baud" value="$(arg baud)" />
|
||||||
</include>
|
</include>
|
||||||
|
|
|
||||||
|
|
@ -49,11 +49,11 @@
|
||||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||||
<buildtool_depend>catkin</buildtool_depend>
|
<buildtool_depend>catkin</buildtool_depend>
|
||||||
<build_depend>mycobot_communication</build_depend>
|
<build_depend>mycobot_320_communication</build_depend>
|
||||||
<build_depend>mycobot_description</build_depend>
|
<build_depend>mycobot_description</build_depend>
|
||||||
<build_export_depend>mycobot_communication</build_export_depend>
|
<build_export_depend>mycobot_320_communication</build_export_depend>
|
||||||
<build_export_depend>mycobot_description</build_export_depend>
|
<build_export_depend>mycobot_description</build_export_depend>
|
||||||
<exec_depend>mycobot_communication</exec_depend>
|
<exec_depend>mycobot_320_communication</exec_depend>
|
||||||
<exec_depend>mycobot_description</exec_depend>
|
<exec_depend>mycobot_description</exec_depend>
|
||||||
<exec_depend>std_msgs</exec_depend>
|
<exec_depend>std_msgs</exec_depend>
|
||||||
<build_depend>std_msgs</build_depend>
|
<build_depend>std_msgs</build_depend>
|
||||||
|
|
|
||||||
216
mycobot_320/mycobot_320_communication/CMakeLists.txt
Normal file
216
mycobot_320/mycobot_320_communication/CMakeLists.txt
Normal file
|
|
@ -0,0 +1,216 @@
|
||||||
|
cmake_minimum_required(VERSION 2.8.3)
|
||||||
|
project(mycobot_320_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/mycobot_services.py
|
||||||
|
scripts/mycobot_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
mycobot_320/mycobot_320_communication/LICENSE
Normal file
25
mycobot_320/mycobot_320_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/ttyUSB0" />
|
||||||
|
<arg name="baud" default="115200" />
|
||||||
|
|
||||||
|
<!-- Open communication service ，开启通讯服务-->
|
||||||
|
<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_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/ttyUSB0" />
|
||||||
|
<arg name="baud" default="115200" />
|
||||||
|
|
||||||
|
<!-- Open communication service ，开启通讯服务-->
|
||||||
|
<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_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/ttyUSB0" />
|
||||||
|
<arg name="baud" default="115200" />
|
||||||
|
|
||||||
|
<!-- Open communication service ，开启通讯服务-->
|
||||||
|
<node name="mycobot_services" pkg="mycobot_320_communication" type="mycobot_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="115200" />
|
||||||
|
|
||||||
|
<!-- Open communication service ，开启通讯服务-->
|
||||||
|
<node name="mycobot_services" pkg="mycobot_320_communication" type="mycobot_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/ttyUSB0" />
|
||||||
|
<arg name="baud" default="115200" />
|
||||||
|
|
||||||
|
<!-- Open communication service，开启通讯服务 -->
|
||||||
|
<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_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/ttyUSB0" />
|
||||||
|
<arg name="baud" default="115200" />
|
||||||
|
|
||||||
|
<!-- Open communication service --><!-- 开启通讯服务 -->
|
||||||
|
<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_topics_pi.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/ttyUSB0" />
|
||||||
|
<arg name="baud" default="115200" />
|
||||||
|
|
||||||
|
<!-- Open communication service ，开启通讯服务-->
|
||||||
|
<node name="mycobot_services" pkg="mycobot_320_communication" type="mycobot_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="115200" />
|
||||||
|
|
||||||
|
<!-- Open communication service ，开启通讯服务-->
|
||||||
|
<node name="mycobot_services" pkg="mycobot_320_communication" type="mycobot_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,6 @@
|
||||||
|
float32 joint_1
|
||||||
|
float32 joint_2
|
||||||
|
float32 joint_3
|
||||||
|
float32 joint_4
|
||||||
|
float32 joint_5
|
||||||
|
float32 joint_6
|
||||||
|
|
@ -0,0 +1,6 @@
|
||||||
|
float32 x
|
||||||
|
float32 y
|
||||||
|
float32 z
|
||||||
|
float32 rx
|
||||||
|
float32 ry
|
||||||
|
float32 rz
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
bool Status
|
||||||
|
|
@ -0,0 +1,3 @@
|
||||||
|
bool Status
|
||||||
|
int8 Pin1
|
||||||
|
int8 Pin2
|
||||||
|
|
@ -0,0 +1,8 @@
|
||||||
|
float32 joint_1
|
||||||
|
float32 joint_2
|
||||||
|
float32 joint_3
|
||||||
|
float32 joint_4
|
||||||
|
float32 joint_5
|
||||||
|
float32 joint_6
|
||||||
|
|
||||||
|
int8 speed
|
||||||
|
|
@ -0,0 +1,9 @@
|
||||||
|
float32 x
|
||||||
|
float32 y
|
||||||
|
float32 z
|
||||||
|
float32 rx
|
||||||
|
float32 ry
|
||||||
|
float32 rz
|
||||||
|
|
||||||
|
int8 speed
|
||||||
|
int8 model
|
||||||
66
mycobot_320/mycobot_320_communication/package.xml
Normal file
66
mycobot_320/mycobot_320_communication/package.xml
Normal file
|
|
@ -0,0 +1,66 @@
|
||||||
|
<?xml version="1.0"?>
|
||||||
|
<package format="2">
|
||||||
|
<name>mycobot_320_communication</name>
|
||||||
|
<version>0.1.0</version>
|
||||||
|
<description>The mycobot_320_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>
|
||||||
|
|
||||||
|
<exec_depend>rospy</exec_depend>
|
||||||
|
<exec_depend>std_msgs</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
mycobot_320/mycobot_320_communication/scripts/mycobot_services.py
Executable file
152
mycobot_320/mycobot_320_communication/scripts/mycobot_services.py
Executable file
|
|
@ -0,0 +1,152 @@
|
||||||
|
#!/usr/bin/env python3
|
||||||
|
# -*- 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("mycobot_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: -170 ~ +170
|
||||||
|
joint 2: -170 ~ +170
|
||||||
|
joint 3: -170 ~ +170
|
||||||
|
joint 4: -170 ~ +170
|
||||||
|
joint 5: -170 ~ +170
|
||||||
|
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
mycobot_320/mycobot_320_communication/scripts/mycobot_topics.py
Executable file
216
mycobot_320/mycobot_320_communication/scripts/mycobot_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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
mycobot_320/mycobot_320_communication/srv/GetAngles.srv
Normal file
9
mycobot_320/mycobot_320_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
mycobot_320/mycobot_320_communication/srv/GetCoords.srv
Normal file
9
mycobot_320/mycobot_320_communication/srv/GetCoords.srv
Normal file
|
|
@ -0,0 +1,9 @@
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
float32 x
|
||||||
|
float32 y
|
||||||
|
float32 z
|
||||||
|
float32 rx
|
||||||
|
float32 ry
|
||||||
|
float32 rz
|
||||||
|
|
@ -0,0 +1,5 @@
|
||||||
|
bool Status
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
bool Flag
|
||||||
7
mycobot_320/mycobot_320_communication/srv/PumpStatus.srv
Normal file
7
mycobot_320/mycobot_320_communication/srv/PumpStatus.srv
Normal file
|
|
@ -0,0 +1,7 @@
|
||||||
|
bool Status
|
||||||
|
int8 Pin1
|
||||||
|
int8 Pin2
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
bool Flag
|
||||||
12
mycobot_320/mycobot_320_communication/srv/SetAngles.srv
Normal file
12
mycobot_320/mycobot_320_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
mycobot_320/mycobot_320_communication/srv/SetCoords.srv
Normal file
13
mycobot_320/mycobot_320_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
|
||||||
|
|
@ -16,7 +16,7 @@
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||||
|
|
||||||
<!-- mycobot-topics -->
|
<!-- mycobot-topics -->
|
||||||
<include file="$(find mycobot_communication)/launch/communication_topic.launch">
|
<include file="$(find mycobot_320_communication)/launch/communication_topic.launch">
|
||||||
<arg name="port" value="$(arg port)" />
|
<arg name="port" value="$(arg port)" />
|
||||||
<arg name="baud" value="$(arg baud)" />
|
<arg name="baud" value="$(arg baud)" />
|
||||||
</include>
|
</include>
|
||||||
|
|
|
||||||
|
|
@ -13,7 +13,7 @@
|
||||||
<!-- Show in Rviz -->
|
<!-- Show in Rviz -->
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||||
|
|
||||||
<include file="$(find mycobot_communication)/launch/communication_service.launch">
|
<include file="$(find mycobot_320_communication)/launch/new_service.launch">
|
||||||
<arg name="port" value="$(arg port)" />
|
<arg name="port" value="$(arg port)" />
|
||||||
<arg name="baud" value="$(arg baud)" />
|
<arg name="baud" value="$(arg baud)" />
|
||||||
</include>
|
</include>
|
||||||
|
|
|
||||||
|
|
@ -13,7 +13,7 @@
|
||||||
<!-- Show in Rviz -->
|
<!-- Show in Rviz -->
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||||
|
|
||||||
<include file="$(find mycobot_communication)/launch/communication_service.launch">
|
<include file="$(find mycobot_320_communication)/launch/new_service.launch">
|
||||||
<arg name="port" value="$(arg port)" />
|
<arg name="port" value="$(arg port)" />
|
||||||
<arg name="baud" value="$(arg baud)" />
|
<arg name="baud" value="$(arg baud)" />
|
||||||
</include>
|
</include>
|
||||||
|
|
|
||||||
|
|
@ -58,11 +58,11 @@
|
||||||
<exec_depend>roscpp</exec_depend>
|
<exec_depend>roscpp</exec_depend>
|
||||||
<exec_depend>rospy</exec_depend> -->
|
<exec_depend>rospy</exec_depend> -->
|
||||||
<exec_depend>std_msgs</exec_depend>
|
<exec_depend>std_msgs</exec_depend>
|
||||||
<build_depend>mycobot_communication</build_depend>
|
<build_depend>mycobot_320_communication</build_depend>
|
||||||
<build_depend>mycobot_description</build_depend>
|
<build_depend>mycobot_description</build_depend>
|
||||||
<build_export_depend>mycobot_communication</build_export_depend>
|
<build_export_depend>mycobot_320_communication</build_export_depend>
|
||||||
<build_export_depend>mycobot_description</build_export_depend>
|
<build_export_depend>mycobot_description</build_export_depend>
|
||||||
<exec_depend>mycobot_communication</exec_depend>
|
<exec_depend>mycobot_320_communication</exec_depend>
|
||||||
<exec_depend>mycobot_description</exec_depend>
|
<exec_depend>mycobot_description</exec_depend>
|
||||||
<exec_depend>actionlib</exec_depend>
|
<exec_depend>actionlib</exec_depend>
|
||||||
<build_depend>actionlib</build_depend>
|
<build_depend>actionlib</build_depend>
|
||||||
|
|
|
||||||
|
|
@ -16,7 +16,7 @@
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||||
|
|
||||||
<!-- mycobot-topics -->
|
<!-- mycobot-topics -->
|
||||||
<include file="$(find mycobot_communication)/launch/communication_topic.launch">
|
<include file="$(find mycobot_320_communication)/launch/communication_topic.launch">
|
||||||
<arg name="port" value="$(arg port)" />
|
<arg name="port" value="$(arg port)" />
|
||||||
<arg name="baud" value="$(arg baud)" />
|
<arg name="baud" value="$(arg baud)" />
|
||||||
</include>
|
</include>
|
||||||
|
|
|
||||||
|
|
@ -13,7 +13,7 @@
|
||||||
<!-- Show in Rviz -->
|
<!-- Show in Rviz -->
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||||
|
|
||||||
<include file="$(find mycobot_communication)/launch/communication_service.launch">
|
<include file="$(find mycobot_320_communication)/launch/new_service_pi.launch">
|
||||||
<arg name="port" value="$(arg port)" />
|
<arg name="port" value="$(arg port)" />
|
||||||
<arg name="baud" value="$(arg baud)" />
|
<arg name="baud" value="$(arg baud)" />
|
||||||
</include>
|
</include>
|
||||||
|
|
|
||||||
|
|
@ -13,7 +13,7 @@
|
||||||
<!-- Show in Rviz -->
|
<!-- Show in Rviz -->
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||||
|
|
||||||
<include file="$(find mycobot_communication)/launch/communication_service.launch">
|
<include file="$(find mycobot_320_communication)/launch/new_service_pi.launch">
|
||||||
<arg name="port" value="$(arg port)" />
|
<arg name="port" value="$(arg port)" />
|
||||||
<arg name="baud" value="$(arg baud)" />
|
<arg name="baud" value="$(arg baud)" />
|
||||||
</include>
|
</include>
|
||||||
|
|
|
||||||
Loading…
Add table
Reference in a new issue