mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-14 09:00:53 +00:00
delete remote directory mycobot_280jn
This commit is contained in:
wangWking
parent
aac3099750
commit
8dcdd29d01
23 changed files with 0 additions and 2082 deletions
|
|
@ -1,40 +0,0 @@
|
||||||
cmake_minimum_required(VERSION 2.8.3)
|
|
||||||
project(mycobot_280jn)
|
|
||||||
add_compile_options(-std=c++11)
|
|
||||||
|
|
||||||
## Find catkin and any catkin packages
|
|
||||||
find_package(catkin REQUIRED COMPONENTS
|
|
||||||
roscpp
|
|
||||||
rospy
|
|
||||||
std_msgs
|
|
||||||
actionlib
|
|
||||||
image_transport
|
|
||||||
cv_bridge
|
|
||||||
)
|
|
||||||
|
|
||||||
## Declare a catkin package
|
|
||||||
catkin_package(
|
|
||||||
CATKIN_DEPENDS std_msgs actionlib
|
|
||||||
)
|
|
||||||
|
|
||||||
## Build talker and listener
|
|
||||||
include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
|
|
||||||
|
|
||||||
catkin_install_python(PROGRAMS
|
|
||||||
scripts/follow_display.py
|
|
||||||
scripts/slider_control.py
|
|
||||||
scripts/teleop_keyboard.py
|
|
||||||
scripts/listen_real.py
|
|
||||||
scripts/listen_real_of_topic.py
|
|
||||||
scripts/detect_marker.py
|
|
||||||
scripts/following_marker.py
|
|
||||||
scripts/follow_and_pump.py
|
|
||||||
scripts/simple_gui.py
|
|
||||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
|
||||||
)
|
|
||||||
|
|
||||||
install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
|
||||||
PATTERN "setup_assistant.launch" EXCLUDE)
|
|
||||||
install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -1,25 +0,0 @@
|
||||||
BSD 2-Clause License
|
|
||||||
|
|
||||||
Copyright (c) 2020, Elephant Robotics
|
|
||||||
All rights reserved.
|
|
||||||
|
|
||||||
Redistribution and use in source and binary forms, with or without
|
|
||||||
modification, are permitted provided that the following conditions are met:
|
|
||||||
|
|
||||||
1. Redistributions of source code must retain the above copyright notice, this
|
|
||||||
list of conditions and the following disclaimer.
|
|
||||||
|
|
||||||
2. Redistributions in binary form must reproduce the above copyright notice,
|
|
||||||
this list of conditions and the following disclaimer in the documentation
|
|
||||||
and/or other materials provided with the distribution.
|
|
||||||
|
|
||||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
|
||||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
||||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
|
||||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
|
||||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
|
||||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
|
||||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
||||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
|
||||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
||||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
|
|
@ -1,203 +0,0 @@
|
||||||
Panels:
|
|
||||||
- Class: rviz/Displays
|
|
||||||
Help Height: 78
|
|
||||||
Name: Displays
|
|
||||||
Property Tree Widget:
|
|
||||||
Expanded:
|
|
||||||
- /Global Options1
|
|
||||||
- /Status1
|
|
||||||
- /RobotModel1
|
|
||||||
- /TF1
|
|
||||||
Splitter Ratio: 0.5
|
|
||||||
Tree Height: 775
|
|
||||||
- Class: rviz/Selection
|
|
||||||
Name: Selection
|
|
||||||
- Class: rviz/Tool Properties
|
|
||||||
Expanded:
|
|
||||||
- /2D Pose Estimate1
|
|
||||||
- /2D Nav Goal1
|
|
||||||
- /Publish Point1
|
|
||||||
Name: Tool Properties
|
|
||||||
Splitter Ratio: 0.588679016
|
|
||||||
- Class: rviz/Views
|
|
||||||
Expanded:
|
|
||||||
- /Current View1
|
|
||||||
Name: Views
|
|
||||||
Splitter Ratio: 0.5
|
|
||||||
- Class: rviz/Time
|
|
||||||
Experimental: false
|
|
||||||
Name: Time
|
|
||||||
SyncMode: 0
|
|
||||||
SyncSource: ""
|
|
||||||
Toolbars:
|
|
||||||
toolButtonStyle: 2
|
|
||||||
Visualization Manager:
|
|
||||||
Class: ""
|
|
||||||
Displays:
|
|
||||||
- Alpha: 0.5
|
|
||||||
Cell Size: 1
|
|
||||||
Class: rviz/Grid
|
|
||||||
Color: 160; 160; 164
|
|
||||||
Enabled: true
|
|
||||||
Line Style:
|
|
||||||
Line Width: 0.0299999993
|
|
||||||
Value: Lines
|
|
||||||
Name: Grid
|
|
||||||
Normal Cell Count: 0
|
|
||||||
Offset:
|
|
||||||
X: 0
|
|
||||||
Y: 0
|
|
||||||
Z: 0
|
|
||||||
Plane: XY
|
|
||||||
Plane Cell Count: 10
|
|
||||||
Reference Frame: <Fixed Frame>
|
|
||||||
Value: true
|
|
||||||
- Alpha: 1
|
|
||||||
Class: rviz/RobotModel
|
|
||||||
Collision Enabled: false
|
|
||||||
Enabled: true
|
|
||||||
Links:
|
|
||||||
All Links Enabled: true
|
|
||||||
Expand Joint Details: false
|
|
||||||
Expand Link Details: false
|
|
||||||
Expand Tree: false
|
|
||||||
Link Tree Style: Links in Alphabetic Order
|
|
||||||
joint1:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
joint2:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
joint3:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
joint4:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
joint5:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
joint6:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
joint6_flange:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
Name: RobotModel
|
|
||||||
Robot Description: robot_description
|
|
||||||
TF Prefix: ""
|
|
||||||
Update Interval: 0
|
|
||||||
Value: true
|
|
||||||
Visual Enabled: true
|
|
||||||
- Class: rviz/TF
|
|
||||||
Enabled: true
|
|
||||||
Frame Timeout: 15
|
|
||||||
Frames:
|
|
||||||
All Enabled: true
|
|
||||||
joint1:
|
|
||||||
Value: true
|
|
||||||
joint2:
|
|
||||||
Value: true
|
|
||||||
joint3:
|
|
||||||
Value: true
|
|
||||||
joint4:
|
|
||||||
Value: true
|
|
||||||
joint5:
|
|
||||||
Value: true
|
|
||||||
joint6:
|
|
||||||
Value: true
|
|
||||||
joint6_flange:
|
|
||||||
Value: true
|
|
||||||
Marker Scale: 0.300000012
|
|
||||||
Name: TF
|
|
||||||
Show Arrows: true
|
|
||||||
Show Axes: true
|
|
||||||
Show Names: true
|
|
||||||
Tree:
|
|
||||||
joint1:
|
|
||||||
joint2:
|
|
||||||
joint3:
|
|
||||||
joint4:
|
|
||||||
joint5:
|
|
||||||
joint6:
|
|
||||||
joint6_flange:
|
|
||||||
{}
|
|
||||||
Update Interval: 0
|
|
||||||
Value: true
|
|
||||||
Enabled: true
|
|
||||||
Global Options:
|
|
||||||
Background Color: 48; 48; 48
|
|
||||||
Default Light: true
|
|
||||||
Fixed Frame: joint1
|
|
||||||
Frame Rate: 30
|
|
||||||
Name: root
|
|
||||||
Tools:
|
|
||||||
- Class: rviz/Interact
|
|
||||||
Hide Inactive Objects: true
|
|
||||||
- Class: rviz/MoveCamera
|
|
||||||
- Class: rviz/Select
|
|
||||||
- Class: rviz/FocusCamera
|
|
||||||
- Class: rviz/Measure
|
|
||||||
- Class: rviz/SetInitialPose
|
|
||||||
Topic: /initialpose
|
|
||||||
- Class: rviz/SetGoal
|
|
||||||
Topic: /move_base_simple/goal
|
|
||||||
- Class: rviz/PublishPoint
|
|
||||||
Single click: true
|
|
||||||
Topic: /clicked_point
|
|
||||||
Value: true
|
|
||||||
Views:
|
|
||||||
Current:
|
|
||||||
Class: rviz/Orbit
|
|
||||||
Distance: 1.20289087
|
|
||||||
Enable Stereo Rendering:
|
|
||||||
Stereo Eye Separation: 0.0599999987
|
|
||||||
Stereo Focal Distance: 1
|
|
||||||
Swap Stereo Eyes: false
|
|
||||||
Value: false
|
|
||||||
Focal Point:
|
|
||||||
X: -0.0706475973
|
|
||||||
Y: -0.0814988762
|
|
||||||
Z: 0.107583851
|
|
||||||
Focal Shape Fixed Size: true
|
|
||||||
Focal Shape Size: 0.0500000007
|
|
||||||
Invert Z Axis: false
|
|
||||||
Name: Current View
|
|
||||||
Near Clip Distance: 0.00999999978
|
|
||||||
Pitch: 0.440398335
|
|
||||||
Target Frame: <Fixed Frame>
|
|
||||||
Value: Orbit (rviz)
|
|
||||||
Yaw: 0.430389732
|
|
||||||
Saved: ~
|
|
||||||
Window Geometry:
|
|
||||||
Displays:
|
|
||||||
collapsed: false
|
|
||||||
Height: 1056
|
|
||||||
Hide Left Dock: false
|
|
||||||
Hide Right Dock: false
|
|
||||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
|
||||||
Selection:
|
|
||||||
collapsed: false
|
|
||||||
Time:
|
|
||||||
collapsed: false
|
|
||||||
Tool Properties:
|
|
||||||
collapsed: false
|
|
||||||
Views:
|
|
||||||
collapsed: false
|
|
||||||
Width: 1855
|
|
||||||
X: 65
|
|
||||||
Y: 24
|
|
||||||
|
|
@ -1,216 +0,0 @@
|
||||||
Panels:
|
|
||||||
- Class: rviz/Displays
|
|
||||||
Help Height: 78
|
|
||||||
Name: Displays
|
|
||||||
Property Tree Widget:
|
|
||||||
Expanded:
|
|
||||||
- /Global Options1
|
|
||||||
- /Status1
|
|
||||||
- /RobotModel1
|
|
||||||
- /TF1
|
|
||||||
- /Marker1
|
|
||||||
- /Marker1/Namespaces1
|
|
||||||
Splitter Ratio: 0.5
|
|
||||||
Tree Height: 775
|
|
||||||
- Class: rviz/Selection
|
|
||||||
Name: Selection
|
|
||||||
- Class: rviz/Tool Properties
|
|
||||||
Expanded:
|
|
||||||
- /2D Pose Estimate1
|
|
||||||
- /2D Nav Goal1
|
|
||||||
- /Publish Point1
|
|
||||||
Name: Tool Properties
|
|
||||||
Splitter Ratio: 0.588679016
|
|
||||||
- Class: rviz/Views
|
|
||||||
Expanded:
|
|
||||||
- /Current View1
|
|
||||||
Name: Views
|
|
||||||
Splitter Ratio: 0.5
|
|
||||||
- Class: rviz/Time
|
|
||||||
Experimental: false
|
|
||||||
Name: Time
|
|
||||||
SyncMode: 0
|
|
||||||
SyncSource: ""
|
|
||||||
Toolbars:
|
|
||||||
toolButtonStyle: 2
|
|
||||||
Visualization Manager:
|
|
||||||
Class: ""
|
|
||||||
Displays:
|
|
||||||
- Alpha: 0.5
|
|
||||||
Cell Size: 1
|
|
||||||
Class: rviz/Grid
|
|
||||||
Color: 160; 160; 164
|
|
||||||
Enabled: true
|
|
||||||
Line Style:
|
|
||||||
Line Width: 0.0299999993
|
|
||||||
Value: Lines
|
|
||||||
Name: Grid
|
|
||||||
Normal Cell Count: 0
|
|
||||||
Offset:
|
|
||||||
X: 0
|
|
||||||
Y: 0
|
|
||||||
Z: 0
|
|
||||||
Plane: XY
|
|
||||||
Plane Cell Count: 10
|
|
||||||
Reference Frame: <Fixed Frame>
|
|
||||||
Value: true
|
|
||||||
- Alpha: 1
|
|
||||||
Class: rviz/RobotModel
|
|
||||||
Collision Enabled: false
|
|
||||||
Enabled: true
|
|
||||||
Links:
|
|
||||||
All Links Enabled: true
|
|
||||||
Expand Joint Details: false
|
|
||||||
Expand Link Details: false
|
|
||||||
Expand Tree: false
|
|
||||||
Link Tree Style: Links in Alphabetic Order
|
|
||||||
joint1:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
joint2:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
joint3:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
joint4:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
joint5:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
joint6:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
joint6_flange:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
Name: RobotModel
|
|
||||||
Robot Description: robot_description
|
|
||||||
TF Prefix: ""
|
|
||||||
Update Interval: 0
|
|
||||||
Value: true
|
|
||||||
Visual Enabled: true
|
|
||||||
- Class: rviz/TF
|
|
||||||
Enabled: true
|
|
||||||
Frame Timeout: 15
|
|
||||||
Frames:
|
|
||||||
All Enabled: true
|
|
||||||
basic_shapes:
|
|
||||||
Value: true
|
|
||||||
joint1:
|
|
||||||
Value: true
|
|
||||||
joint2:
|
|
||||||
Value: true
|
|
||||||
joint3:
|
|
||||||
Value: true
|
|
||||||
joint4:
|
|
||||||
Value: true
|
|
||||||
joint5:
|
|
||||||
Value: true
|
|
||||||
joint6:
|
|
||||||
Value: true
|
|
||||||
joint6_flange:
|
|
||||||
Value: true
|
|
||||||
Marker Scale: 0.300000012
|
|
||||||
Name: TF
|
|
||||||
Show Arrows: true
|
|
||||||
Show Axes: true
|
|
||||||
Show Names: true
|
|
||||||
Tree:
|
|
||||||
joint1:
|
|
||||||
joint2:
|
|
||||||
joint3:
|
|
||||||
joint4:
|
|
||||||
joint5:
|
|
||||||
joint6:
|
|
||||||
joint6_flange:
|
|
||||||
basic_shapes:
|
|
||||||
{}
|
|
||||||
Update Interval: 0
|
|
||||||
Value: true
|
|
||||||
- Class: rviz/Marker
|
|
||||||
Enabled: true
|
|
||||||
Marker Topic: /visualization_marker
|
|
||||||
Name: Marker
|
|
||||||
Namespaces:
|
|
||||||
basic_cube: true
|
|
||||||
Queue Size: 100
|
|
||||||
Value: true
|
|
||||||
Enabled: true
|
|
||||||
Global Options:
|
|
||||||
Background Color: 48; 48; 48
|
|
||||||
Default Light: true
|
|
||||||
Fixed Frame: joint1
|
|
||||||
Frame Rate: 30
|
|
||||||
Name: root
|
|
||||||
Tools:
|
|
||||||
- Class: rviz/Interact
|
|
||||||
Hide Inactive Objects: true
|
|
||||||
- Class: rviz/MoveCamera
|
|
||||||
- Class: rviz/Select
|
|
||||||
- Class: rviz/FocusCamera
|
|
||||||
- Class: rviz/Measure
|
|
||||||
- Class: rviz/SetInitialPose
|
|
||||||
Topic: /initialpose
|
|
||||||
- Class: rviz/SetGoal
|
|
||||||
Topic: /move_base_simple/goal
|
|
||||||
- Class: rviz/PublishPoint
|
|
||||||
Single click: true
|
|
||||||
Topic: /clicked_point
|
|
||||||
Value: true
|
|
||||||
Views:
|
|
||||||
Current:
|
|
||||||
Class: rviz/Orbit
|
|
||||||
Distance: 2.11990476
|
|
||||||
Enable Stereo Rendering:
|
|
||||||
Stereo Eye Separation: 0.0599999987
|
|
||||||
Stereo Focal Distance: 1
|
|
||||||
Swap Stereo Eyes: false
|
|
||||||
Value: false
|
|
||||||
Focal Point:
|
|
||||||
X: -0.0706475973
|
|
||||||
Y: -0.0814988762
|
|
||||||
Z: 0.107583851
|
|
||||||
Focal Shape Fixed Size: true
|
|
||||||
Focal Shape Size: 0.0500000007
|
|
||||||
Invert Z Axis: false
|
|
||||||
Name: Current View
|
|
||||||
Near Clip Distance: 0.00999999978
|
|
||||||
Pitch: 0.375398338
|
|
||||||
Target Frame: <Fixed Frame>
|
|
||||||
Value: Orbit (rviz)
|
|
||||||
Yaw: 0.235389769
|
|
||||||
Saved: ~
|
|
||||||
Window Geometry:
|
|
||||||
Displays:
|
|
||||||
collapsed: false
|
|
||||||
Height: 1056
|
|
||||||
Hide Left Dock: false
|
|
||||||
Hide Right Dock: false
|
|
||||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
|
||||||
Selection:
|
|
||||||
collapsed: false
|
|
||||||
Time:
|
|
||||||
collapsed: false
|
|
||||||
Tool Properties:
|
|
||||||
collapsed: false
|
|
||||||
Views:
|
|
||||||
collapsed: false
|
|
||||||
Width: 1855
|
|
||||||
X: 65
|
|
||||||
Y: 24
|
|
||||||
|
|
@ -1,18 +0,0 @@
|
||||||
<launch>
|
|
||||||
<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
|
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/280jn/mycobot_urdf.urdf"/>
|
|
||||||
<arg name="rvizconfig" default="$(find mycobot_280jn)/config/mycobot_with_marker.rviz" />
|
|
||||||
<arg name="gui" default="true" />
|
|
||||||
|
|
||||||
<arg name="num" default="0" />
|
|
||||||
<!-- Add model control,添加模型控制 -->
|
|
||||||
<include file="$(find mycobot_280jn)/launch/slider_control.launch">
|
|
||||||
<arg name="model" value="$(arg model)" />
|
|
||||||
<arg name="rvizconfig" value="$(arg rvizconfig)" />
|
|
||||||
<arg name="gui" value="$(arg gui)" />
|
|
||||||
</include>
|
|
||||||
<!-- vision node -->
|
|
||||||
<node name="opencv_camera" pkg="mycobot_280jn" type="opencv_camera" args="$(arg num)"/>
|
|
||||||
<node name="detect_marker" pkg="mycobot_280jn" type="detect_marker.py" />
|
|
||||||
<node name="following_marker" pkg="mycobot_280jn" type="following_marker.py" />
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,30 +0,0 @@
|
||||||
<launch>
|
|
||||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
|
||||||
<arg name="port" default="/dev/ttyTHS1" />
|
|
||||||
<arg name="baud" default="1000000" />
|
|
||||||
<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
|
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/280jn/mycobot_urdf.urdf"/>
|
|
||||||
<arg name="rvizconfig" default="$(find mycobot_280jn)/config/mycobot_with_marker.rviz" />
|
|
||||||
<arg name="gui" default="false" />
|
|
||||||
|
|
||||||
<arg name="num" default="0" />
|
|
||||||
|
|
||||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
|
||||||
|
|
||||||
<!-- Combinejoin values to TF ，将值合并到TF-->
|
|
||||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
|
||||||
<!-- Show in Rviz，显示在Rviz -->
|
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
|
||||||
|
|
||||||
<!-- mycobot-topics， mycobot-话题-->
|
|
||||||
<include file="$(find mycobot_communication)/launch/communication_topic.launch">
|
|
||||||
<arg name="port" value="$(arg port)" />
|
|
||||||
<arg name="baud" value="$(arg baud)" />
|
|
||||||
</include>
|
|
||||||
<!-- listen and pub the real angles ，监听并发布真实的角度-->
|
|
||||||
<node name="real_listener" pkg="mycobot_280jn" type="listen_real_of_topic.py" />
|
|
||||||
<!-- vision node -->
|
|
||||||
<node name="opencv_camera" pkg="mycobot_280jn" type="opencv_camera" args="$(arg num)"/>
|
|
||||||
<node name="detect_marker" pkg="mycobot_280jn" type="detect_marker.py" />
|
|
||||||
<node name="following_marker" pkg="mycobot_280jn" type="following_marker.py" />
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,13 +0,0 @@
|
||||||
<launch>
|
|
||||||
<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
|
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/280jn/mycobot_urdf.urdf"/>
|
|
||||||
<arg name="rvizconfig" default="$(find mycobot_280jn)/config/mycobot.rviz" />
|
|
||||||
|
|
||||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
|
||||||
<!-- mycobot-topics，mycobot-话题 -->
|
|
||||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" >
|
|
||||||
<rosparam param="source_list" subst_value="true">["joint_states"]</rosparam>
|
|
||||||
</node>
|
|
||||||
<!-- Show in Rviz，显示在Rviz -->
|
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,24 +0,0 @@
|
||||||
<launch>
|
|
||||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
|
||||||
<arg name="port" default="/dev/ttyTHS1" />
|
|
||||||
<arg name="baud" default="1000000" />
|
|
||||||
<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
|
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/280jn/mycobot_urdf.urdf"/>
|
|
||||||
<arg name="rvizconfig" default="$(find mycobot_280jn)/config/mycobot.rviz" />
|
|
||||||
<arg name="gui" default="false" />
|
|
||||||
|
|
||||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
|
||||||
|
|
||||||
<!-- Combinejoin values to TF ，将值合并到TF-->
|
|
||||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
|
||||||
<!-- Show in Rviz ，显示在Rviz-->
|
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
|
||||||
|
|
||||||
<include file="$(find mycobot_communication)/launch/communication_service.launch">
|
|
||||||
<arg name="port" value="$(arg port)" />
|
|
||||||
<arg name="baud" value="$(arg baud)" />
|
|
||||||
</include>
|
|
||||||
<!-- listen and pub the real angles ，监听并发布真实的角度-->
|
|
||||||
<node name="real_listener" pkg="mycobot_280jn" type="listen_real.py" />
|
|
||||||
<node name="simple_gui" pkg="mycobot_280jn" type="simple_gui.py" />
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,24 +0,0 @@
|
||||||
<launch>
|
|
||||||
<!-- <arg name="port" default="/dev/ttyTHS1" />
|
|
||||||
<arg name="baud" default="1000000" /> -->
|
|
||||||
<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
|
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/280jn/mycobot_urdf.urdf"/>
|
|
||||||
<arg name="rvizconfig" default="$(find mycobot_280jn)/config/mycobot.rviz" />
|
|
||||||
<arg name="gui" default="true" />
|
|
||||||
|
|
||||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
|
||||||
|
|
||||||
<!-- Combinejoin values to TF. 将值合并到TF-->
|
|
||||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
|
||||||
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui">
|
|
||||||
<!-- <param name="use_gui" value="$(arg gui)" /> -->
|
|
||||||
<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
|
|
||||||
</node>
|
|
||||||
<!-- Open control script -->
|
|
||||||
<!-- <node name="control_slider" pkg="mycobot_280jn" type="slider_control.py">
|
|
||||||
<param name="port" type="string" value="$(arg port)" />
|
|
||||||
<param name="baud" type="int" value="$(arg baud)" />
|
|
||||||
</node> -->
|
|
||||||
<!-- Show in Rviz .显示在Rviz-->
|
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,23 +0,0 @@
|
||||||
<launch>
|
|
||||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
|
||||||
<arg name="port" default="/dev/ttyTHS1" />
|
|
||||||
<arg name="baud" default="1000000" />
|
|
||||||
<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
|
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/280jn/mycobot_urdf.urdf"/>
|
|
||||||
<arg name="rvizconfig" default="$(find mycobot_280jn)/config/mycobot.rviz" />
|
|
||||||
<arg name="gui" default="false" />
|
|
||||||
|
|
||||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
|
||||||
|
|
||||||
<!-- Combinejoin values to TF ，将值合并到TF-->
|
|
||||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
|
||||||
<!-- Show in Rviz ，显示在Rviz-->
|
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
|
||||||
|
|
||||||
<include file="$(find mycobot_communication)/launch/communication_service.launch">
|
|
||||||
<arg name="port" value="$(arg port)" />
|
|
||||||
<arg name="baud" value="$(arg baud)" />
|
|
||||||
</include>
|
|
||||||
<!-- listen and pub the real angles ，监听并发布真实的角度-->
|
|
||||||
<node name="real_listener" pkg="mycobot_280jn" type="listen_real.py" />
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,17 +0,0 @@
|
||||||
<launch>
|
|
||||||
<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 -->
|
|
||||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_with_vision.urdf"/>
|
|
||||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot.rviz" />
|
|
||||||
<arg name="gui" default="true" />
|
|
||||||
|
|
||||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
|
||||||
|
|
||||||
<!-- Combinejoin values to TF ,将值合并到TF-->
|
|
||||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
|
||||||
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
|
|
||||||
<param name="use_gui" value="$(arg gui)" />
|
|
||||||
<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
|
|
||||||
</node>
|
|
||||||
<!-- Show in Rviz ，显示在Rviz-->
|
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,47 +0,0 @@
|
||||||
<?xml version="1.0"?>
|
|
||||||
<package format="2">
|
|
||||||
<name>mycobot_280jn</name>
|
|
||||||
<version>0.3.0</version>
|
|
||||||
<description>The mycobot 280jn package</description>
|
|
||||||
|
|
||||||
<author email="lijun.zhang@elephantrobotics.com">ZhangLijun</author>
|
|
||||||
<maintainer email="lijun.zhang@elephantrobotics.com">ZhangLijun</maintainer>
|
|
||||||
|
|
||||||
<license>BSD</license>
|
|
||||||
|
|
||||||
<url type="website">https://github.com/elephantrobotics/mycobot_ros</url>
|
|
||||||
|
|
||||||
<buildtool_depend>catkin</buildtool_depend>
|
|
||||||
|
|
||||||
<build_depend>roscpp</build_depend>
|
|
||||||
<build_depend>rospy</build_depend>
|
|
||||||
<build_depend>std_msgs</build_depend>
|
|
||||||
<build_depend>actionlib</build_depend>
|
|
||||||
<build_depend>mycobot_description</build_depend>
|
|
||||||
<build_depend>mycobot_communication</build_depend>
|
|
||||||
|
|
||||||
<build_export_depend>mycobot_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>mycobot_communication</exec_depend>
|
|
||||||
|
|
||||||
|
|
||||||
<!-- The export tag contains other, unspecified, tags -->
|
|
||||||
<export>
|
|
||||||
<!-- Other tools can request additional information be placed here -->
|
|
||||||
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
|
|
@ -1,123 +0,0 @@
|
||||||
#!/usr/bin/env python
|
|
||||||
import rospy
|
|
||||||
import cv2 as cv
|
|
||||||
import numpy as np
|
|
||||||
from cv_bridge import CvBridge, CvBridgeError
|
|
||||||
from sensor_msgs.msg import Image
|
|
||||||
import tf
|
|
||||||
from tf.broadcaster import TransformBroadcaster
|
|
||||||
import tf_conversions
|
|
||||||
from mycobot_communication.srv import (
|
|
||||||
GetCoords,
|
|
||||||
SetCoords,
|
|
||||||
GetAngles,
|
|
||||||
SetAngles,
|
|
||||||
GripperStatus,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
class ImageConverter:
|
|
||||||
def __init__(self):
|
|
||||||
self.br = TransformBroadcaster()
|
|
||||||
self.bridge = CvBridge()
|
|
||||||
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
|
|
||||||
self.aruo_params = cv.aruco.DetectorParameters_create()
|
|
||||||
calibrationParams = cv.FileStorage(
|
|
||||||
"calibrationFileName.xml", cv.FILE_STORAGE_READ
|
|
||||||
)
|
|
||||||
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
|
|
||||||
self.camera_matrix = None
|
|
||||||
# subscriber, listen wether has img come in. 订阅者，监听是否有img
|
|
||||||
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
|
|
||||||
|
|
||||||
def callback(self, data):
|
|
||||||
"""Callback function.
|
|
||||||
|
|
||||||
Process image with OpenCV, detect Mark to get the pose. Then acccording the
|
|
||||||
pose to transforming.
|
|
||||||
"""
|
|
||||||
try:
|
|
||||||
# trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
|
|
||||||
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
|
|
||||||
except CvBridgeError as e:
|
|
||||||
print(e)
|
|
||||||
size = cv_image.shape
|
|
||||||
focal_length = size[1]
|
|
||||||
center = [size[1] / 2, size[0] / 2]
|
|
||||||
if self.camera_matrix is None:
|
|
||||||
# calc the camera matrix, if don't have.如果没有，则计算相机矩阵
|
|
||||||
self.camera_matrix = np.array(
|
|
||||||
[
|
|
||||||
[focal_length, 0, center[0]],
|
|
||||||
[0, focal_length, center[1]],
|
|
||||||
[0, 0, 1],
|
|
||||||
],
|
|
||||||
dtype=np.float32,
|
|
||||||
)
|
|
||||||
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
|
|
||||||
# detect aruco marker.检测 aruco 标记
|
|
||||||
ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params)
|
|
||||||
corners, ids = ret[0], ret[1]
|
|
||||||
# process marker data.处理标记数据
|
|
||||||
if len(corners) > 0:
|
|
||||||
if ids is not None:
|
|
||||||
# print('corners:', corners, 'ids:', ids)
|
|
||||||
|
|
||||||
# detect marker pose. 检测marker位姿。
|
|
||||||
# argument:
|
|
||||||
# marker corners,标记角
|
|
||||||
# marker size (meter),标记尺寸（米）
|
|
||||||
ret = cv.aruco.estimatePoseSingleMarkers(
|
|
||||||
corners, 0.05, self.camera_matrix, self.dist_coeffs
|
|
||||||
)
|
|
||||||
(rvec, tvec) = (ret[0], ret[1])
|
|
||||||
(rvec - tvec).any()
|
|
||||||
|
|
||||||
print("rvec:", rvec, "tvec:", tvec)
|
|
||||||
|
|
||||||
# just select first one detected marker.只需选择第一个检测到的标记。
|
|
||||||
for i in range(rvec.shape[0]):
|
|
||||||
cv.aruco.drawDetectedMarkers(cv_image, corners)
|
|
||||||
cv.aruco.drawAxis(
|
|
||||||
cv_image,
|
|
||||||
self.camera_matrix,
|
|
||||||
self.dist_coeffs,
|
|
||||||
rvec[i, :, :],
|
|
||||||
tvec[i, :, :],
|
|
||||||
0.03,
|
|
||||||
)
|
|
||||||
|
|
||||||
xyz = tvec[0, 0, :]
|
|
||||||
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
|
|
||||||
|
|
||||||
# get quaternion for ros. 为ros获取四元数
|
|
||||||
euler = rvec[0, 0, :]
|
|
||||||
tf_change = tf.transformations.quaternion_from_euler(
|
|
||||||
euler[0], euler[1], euler[2]
|
|
||||||
)
|
|
||||||
print("tf_change:", tf_change)
|
|
||||||
|
|
||||||
# trans pose according [joint1]，根据 [joint1] 变换姿势
|
|
||||||
self.br.sendTransform(
|
|
||||||
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
|
|
||||||
)
|
|
||||||
|
|
||||||
# [x, y, z, -172, 3, -46.8]
|
|
||||||
cv.imshow("Image", cv_image)
|
|
||||||
|
|
||||||
cv.waitKey(3)
|
|
||||||
try:
|
|
||||||
pass
|
|
||||||
except CvBridgeError as e:
|
|
||||||
print(e)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
try:
|
|
||||||
rospy.init_node("detect_marker")
|
|
||||||
rospy.loginfo("Starting cv_bridge_test node")
|
|
||||||
ImageConverter()
|
|
||||||
rospy.spin()
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print("Shutting down cv_bridge_test node.")
|
|
||||||
cv.destroyAllWindows()
|
|
||||||
|
|
@ -1,194 +0,0 @@
|
||||||
#!/usr/bin/env python2
|
|
||||||
# coding:utf-8
|
|
||||||
import rospy
|
|
||||||
from visualization_msgs.msg import Marker
|
|
||||||
import time
|
|
||||||
import os
|
|
||||||
|
|
||||||
# Type of message communicated with mycobot，与 mycobot 通信的消息类型
|
|
||||||
from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
|
|
||||||
|
|
||||||
|
|
||||||
rospy.init_node("gipper_subscriber", anonymous=True)
|
|
||||||
|
|
||||||
# Control the topic of mycobot, followed by angle, coordinates, gripper
|
|
||||||
# 控制 mycobot 的 topic，依次是角度、坐标、夹爪
|
|
||||||
angle_pub = rospy.Publisher("mycobot/angles_goal",
|
|
||||||
MycobotSetAngles, queue_size=5)
|
|
||||||
coord_pub = rospy.Publisher("mycobot/coords_goal",
|
|
||||||
MycobotSetCoords, queue_size=5)
|
|
||||||
# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio
|
|
||||||
robot = os.popen("ls /dev/ttyUSB*").readline()
|
|
||||||
|
|
||||||
if "dev" in robot:
|
|
||||||
Pin = [2, 5]
|
|
||||||
else:
|
|
||||||
Pin = [20, 21]
|
|
||||||
|
|
||||||
pump_pub = rospy.Publisher("mycobot/pump_status",
|
|
||||||
MycobotPumpStatus, queue_size=5)
|
|
||||||
|
|
||||||
# instantiate the message object，实例化消息对象
|
|
||||||
angles = MycobotSetAngles()
|
|
||||||
coords = MycobotSetCoords()
|
|
||||||
pump = MycobotPumpStatus()
|
|
||||||
|
|
||||||
# Deviation value from mycobot's real position,与 mycobot 真实位置的偏差值
|
|
||||||
x_offset = -20
|
|
||||||
y_offset = 20
|
|
||||||
z_offset = 110
|
|
||||||
|
|
||||||
# With this variable limit, the fetching behavior is only done once
|
|
||||||
# 通过该变量限制，抓取行为只做一次
|
|
||||||
flag = False
|
|
||||||
|
|
||||||
# In order to compare whether the QR code moves later,为了后面比较二维码是否移动
|
|
||||||
temp_x = temp_y = temp_z = 0.0
|
|
||||||
|
|
||||||
temp_time = time.time()
|
|
||||||
|
|
||||||
|
|
||||||
def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2):
|
|
||||||
"""Post coordinates,发布坐标"""
|
|
||||||
coords.x = x
|
|
||||||
coords.y = y
|
|
||||||
coords.z = z
|
|
||||||
coords.rx = rx
|
|
||||||
coords.ry = ry
|
|
||||||
coords.rz = rz
|
|
||||||
coords.speed = 70
|
|
||||||
coords.model = m
|
|
||||||
# print(coords)
|
|
||||||
coord_pub.publish(coords)
|
|
||||||
|
|
||||||
|
|
||||||
def pub_angles(a, b, c, d, e, f, sp):
|
|
||||||
"""Publishing angle,发布角度"""
|
|
||||||
angles.joint_1 = float(a)
|
|
||||||
angles.joint_2 = float(b)
|
|
||||||
angles.joint_3 = float(c)
|
|
||||||
angles.joint_4 = float(d)
|
|
||||||
angles.joint_5 = float(e)
|
|
||||||
angles.joint_6 = float(f)
|
|
||||||
angles.speed = sp
|
|
||||||
angle_pub.publish(angles)
|
|
||||||
|
|
||||||
|
|
||||||
def pub_pump(flag, Pin):
|
|
||||||
"""Publish gripper status,发布夹爪状态"""
|
|
||||||
pump.Status = flag
|
|
||||||
pump.Pin1 = Pin[0]
|
|
||||||
pump.Pin2 = Pin[1]
|
|
||||||
pump_pub.publish(pump)
|
|
||||||
|
|
||||||
|
|
||||||
def target_is_moving(x, y, z):
|
|
||||||
"""Determine whether the target moves"""
|
|
||||||
"""判断目标是否移动"""
|
|
||||||
count = 0
|
|
||||||
for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
|
|
||||||
print(o, n)
|
|
||||||
if abs(o - n) < 2:
|
|
||||||
count += 1
|
|
||||||
print(count)
|
|
||||||
if count == 3:
|
|
||||||
return False
|
|
||||||
return True
|
|
||||||
|
|
||||||
|
|
||||||
def grippercallback(data):
|
|
||||||
"""callback function,回调函数"""
|
|
||||||
global flag, temp_x, temp_y, temp_z
|
|
||||||
# rospy.loginfo('gripper_subscriber get date :%s', data)
|
|
||||||
if flag:
|
|
||||||
return
|
|
||||||
|
|
||||||
# Parse out the coordinate value,解析出坐标值
|
|
||||||
# pump length: 88mm
|
|
||||||
x = float(format(data.pose.position.x * 1000, ".2f"))
|
|
||||||
y = float(format(data.pose.position.y * 1000, ".2f"))
|
|
||||||
z = float(format(data.pose.position.z * 1000, ".2f"))
|
|
||||||
|
|
||||||
# When the running time is less than 30s, or the target position is still changing, perform tracking behavior
|
|
||||||
# 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为
|
|
||||||
if (
|
|
||||||
time.time() - temp_time < 30
|
|
||||||
or (temp_x == temp_y == temp_z == 0.0)
|
|
||||||
or target_is_moving(x - x_offset, y - y_offset, z)
|
|
||||||
):
|
|
||||||
|
|
||||||
x -= x_offset
|
|
||||||
y -= y_offset
|
|
||||||
pub_coords(x - 20, y, 280)
|
|
||||||
time.sleep(0.1)
|
|
||||||
|
|
||||||
temp_x, temp_y, temp_z = x, y, z
|
|
||||||
return
|
|
||||||
else: #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取
|
|
||||||
|
|
||||||
print(x, y, z)
|
|
||||||
|
|
||||||
# detect heigth + pump height + limit height + offset
|
|
||||||
x += x_offset
|
|
||||||
y += y_offset
|
|
||||||
z = z + 88 + z_offset
|
|
||||||
|
|
||||||
pub_coords(x, y, z)
|
|
||||||
time.sleep(2.5)
|
|
||||||
|
|
||||||
# down
|
|
||||||
for i in range(1, 17):
|
|
||||||
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
|
|
||||||
time.sleep(0.1)
|
|
||||||
|
|
||||||
time.sleep(2)
|
|
||||||
|
|
||||||
pub_pump(True, Pin)
|
|
||||||
# pump on
|
|
||||||
|
|
||||||
pub_coords(x, y, z + 20, -165)
|
|
||||||
time.sleep(1.5)
|
|
||||||
|
|
||||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
|
||||||
time.sleep(1.5)
|
|
||||||
|
|
||||||
put_z = 140
|
|
||||||
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
|
|
||||||
time.sleep(1.5)
|
|
||||||
|
|
||||||
for i in range(1, 8):
|
|
||||||
pub_coords(39.4, -174.7, put_z - i * 5, -
|
|
||||||
177.13, -4.13, -152.59, 15, 2)
|
|
||||||
time.sleep(0.1)
|
|
||||||
|
|
||||||
pub_pump(False, Pin)
|
|
||||||
|
|
||||||
time.sleep(0.5)
|
|
||||||
|
|
||||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
|
||||||
time.sleep(1.5)
|
|
||||||
|
|
||||||
# finally
|
|
||||||
flag = True
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
for _ in range(10):
|
|
||||||
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
|
|
||||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
|
||||||
# pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70)
|
|
||||||
time.sleep(0.5)
|
|
||||||
|
|
||||||
pub_pump(False, Pin)
|
|
||||||
# time.sleep(2.5)
|
|
||||||
|
|
||||||
# subscribers to mark information，mark 信息的订阅者
|
|
||||||
rospy.Subscriber("visualization_marker", Marker,
|
|
||||||
grippercallback, queue_size=1)
|
|
||||||
|
|
||||||
print("gripper test")
|
|
||||||
rospy.spin()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
|
|
@ -1,103 +0,0 @@
|
||||||
#!/usr/bin/env python2
|
|
||||||
import time
|
|
||||||
|
|
||||||
import rospy
|
|
||||||
from sensor_msgs.msg import JointState
|
|
||||||
from std_msgs.msg import Header
|
|
||||||
from visualization_msgs.msg import Marker
|
|
||||||
|
|
||||||
from pymycobot.mycobot import MyCobot
|
|
||||||
|
|
||||||
|
|
||||||
def talker():
|
|
||||||
rospy.init_node("display", anonymous=True)
|
|
||||||
|
|
||||||
print("Try connect real mycobot...")
|
|
||||||
port = rospy.get_param("~port", "/dev/ttyTHS1")
|
|
||||||
baud = rospy.get_param("~baud", 1000000)
|
|
||||||
print("port: {}, baud: {}\n".format(port, baud))
|
|
||||||
try:
|
|
||||||
mycobot = MyCobot(port, baud)
|
|
||||||
except Exception as e:
|
|
||||||
print(e)
|
|
||||||
print(
|
|
||||||
"""\
|
|
||||||
\rTry connect mycobot failed!
|
|
||||||
\rPlease check wether connected with mycobot.
|
|
||||||
\rPlease chckt wether the port or baud is right.
|
|
||||||
"""
|
|
||||||
)
|
|
||||||
exit(1)
|
|
||||||
mycobot.release_all_servos()
|
|
||||||
time.sleep(0.1)
|
|
||||||
print("Rlease all servos over.\n")
|
|
||||||
|
|
||||||
pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
|
||||||
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
|
|
||||||
rate = rospy.Rate(30) # 30hz
|
|
||||||
|
|
||||||
# pub joint state
|
|
||||||
joint_state_send = JointState()
|
|
||||||
joint_state_send.header = Header()
|
|
||||||
|
|
||||||
joint_state_send.name = [
|
|
||||||
"joint2_to_joint1",
|
|
||||||
"joint3_to_joint2",
|
|
||||||
"joint4_to_joint3",
|
|
||||||
"joint5_to_joint4",
|
|
||||||
"joint6_to_joint5",
|
|
||||||
"joint6output_to_joint6",
|
|
||||||
]
|
|
||||||
joint_state_send.velocity = [0]
|
|
||||||
joint_state_send.effort = []
|
|
||||||
|
|
||||||
marker_ = Marker()
|
|
||||||
marker_.header.frame_id = "/joint1"
|
|
||||||
marker_.ns = "my_namespace"
|
|
||||||
|
|
||||||
print("publishing ...")
|
|
||||||
while not rospy.is_shutdown():
|
|
||||||
joint_state_send.header.stamp = rospy.Time.now()
|
|
||||||
|
|
||||||
angles = mycobot.get_radians()
|
|
||||||
data_list = []
|
|
||||||
for index, value in enumerate(angles):
|
|
||||||
data_list.append(value)
|
|
||||||
|
|
||||||
# rospy.loginfo('{}'.format(data_list))
|
|
||||||
joint_state_send.position = data_list
|
|
||||||
|
|
||||||
pub.publish(joint_state_send)
|
|
||||||
|
|
||||||
coords = mycobot.get_coords()
|
|
||||||
|
|
||||||
# marker
|
|
||||||
marker_.header.stamp = rospy.Time.now()
|
|
||||||
marker_.type = marker_.SPHERE
|
|
||||||
marker_.action = marker_.ADD
|
|
||||||
marker_.scale.x = 0.04
|
|
||||||
marker_.scale.y = 0.04
|
|
||||||
marker_.scale.z = 0.04
|
|
||||||
|
|
||||||
# marker position initial.标记位置初始
|
|
||||||
# print(coords)
|
|
||||||
if not coords:
|
|
||||||
coords = [0, 0, 0, 0, 0, 0]
|
|
||||||
rospy.loginfo("error [101]: can not get coord values")
|
|
||||||
|
|
||||||
marker_.pose.position.x = coords[1] / 1000 * -1
|
|
||||||
marker_.pose.position.y = coords[0] / 1000
|
|
||||||
marker_.pose.position.z = coords[2] / 1000
|
|
||||||
|
|
||||||
marker_.color.a = 1.0
|
|
||||||
marker_.color.g = 1.0
|
|
||||||
pub_marker.publish(marker_)
|
|
||||||
|
|
||||||
rate.sleep()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
try:
|
|
||||||
talker()
|
|
||||||
except rospy.ROSInterruptException:
|
|
||||||
pass
|
|
||||||
|
|
@ -1,64 +0,0 @@
|
||||||
#!/usr/bin/env python2
|
|
||||||
import time
|
|
||||||
|
|
||||||
import rospy
|
|
||||||
from sensor_msgs.msg import JointState
|
|
||||||
from std_msgs.msg import Header
|
|
||||||
from visualization_msgs.msg import Marker
|
|
||||||
import tf
|
|
||||||
|
|
||||||
|
|
||||||
def talker():
|
|
||||||
rospy.init_node("following_marker", anonymous=True)
|
|
||||||
|
|
||||||
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
|
|
||||||
rate = rospy.Rate(20)
|
|
||||||
|
|
||||||
listener = tf.TransformListener()
|
|
||||||
|
|
||||||
marker_ = Marker()
|
|
||||||
marker_.header.frame_id = "/joint1"
|
|
||||||
marker_.ns = "basic_cube"
|
|
||||||
|
|
||||||
print("publishing ...")
|
|
||||||
while not rospy.is_shutdown():
|
|
||||||
now = rospy.Time.now() - rospy.Duration(0.1)
|
|
||||||
|
|
||||||
try:
|
|
||||||
trans, rot = listener.lookupTransform("joint1", "basic_shapes", now)
|
|
||||||
except Exception as e:
|
|
||||||
print(e)
|
|
||||||
continue
|
|
||||||
|
|
||||||
print(type(trans), trans)
|
|
||||||
print(type(rot), rot)
|
|
||||||
|
|
||||||
# marker
|
|
||||||
marker_.header.stamp = now
|
|
||||||
marker_.type = marker_.CUBE
|
|
||||||
marker_.action = marker_.ADD
|
|
||||||
marker_.scale.x = 0.04
|
|
||||||
marker_.scale.y = 0.04
|
|
||||||
marker_.scale.z = 0.04
|
|
||||||
|
|
||||||
# marker position initial,标记位置初始化
|
|
||||||
marker_.pose.position.x = trans[0]
|
|
||||||
marker_.pose.position.y = trans[1]
|
|
||||||
marker_.pose.position.z = trans[2]
|
|
||||||
marker_.pose.orientation.x = rot[0]
|
|
||||||
marker_.pose.orientation.y = rot[1]
|
|
||||||
marker_.pose.orientation.z = rot[2]
|
|
||||||
marker_.pose.orientation.w = rot[3]
|
|
||||||
|
|
||||||
marker_.color.a = 1.0
|
|
||||||
marker_.color.g = 1.0
|
|
||||||
pub_marker.publish(marker_)
|
|
||||||
|
|
||||||
rate.sleep()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
try:
|
|
||||||
talker()
|
|
||||||
except rospy.ROSInterruptException:
|
|
||||||
pass
|
|
||||||
|
|
@ -1,65 +0,0 @@
|
||||||
#!/usr/bin/env python2
|
|
||||||
# license removed for brevity
|
|
||||||
import time
|
|
||||||
import math
|
|
||||||
|
|
||||||
import rospy
|
|
||||||
from sensor_msgs.msg import JointState
|
|
||||||
from std_msgs.msg import Header
|
|
||||||
from mycobot_communication.srv import GetAngles
|
|
||||||
|
|
||||||
|
|
||||||
def talker():
|
|
||||||
rospy.loginfo("start ...")
|
|
||||||
rospy.init_node("real_listener", anonymous=True)
|
|
||||||
pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
|
||||||
rate = rospy.Rate(30) # 30hz
|
|
||||||
|
|
||||||
# pub joint state，发布关节状态
|
|
||||||
joint_state_send = JointState()
|
|
||||||
joint_state_send.header = Header()
|
|
||||||
|
|
||||||
joint_state_send.name = [
|
|
||||||
"joint2_to_joint1",
|
|
||||||
"joint3_to_joint2",
|
|
||||||
"joint4_to_joint3",
|
|
||||||
"joint5_to_joint4",
|
|
||||||
"joint6_to_joint5",
|
|
||||||
"joint6output_to_joint6",
|
|
||||||
]
|
|
||||||
joint_state_send.velocity = [0]
|
|
||||||
joint_state_send.effort = []
|
|
||||||
|
|
||||||
# waiting util server `get_joint_angles` enable.等待'get_joint_angles'服务启用
|
|
||||||
rospy.loginfo("wait service")
|
|
||||||
rospy.wait_for_service("get_joint_angles")
|
|
||||||
func = rospy.ServiceProxy("get_joint_angles", GetAngles)
|
|
||||||
|
|
||||||
rospy.loginfo("start loop ...")
|
|
||||||
while not rospy.is_shutdown():
|
|
||||||
# get real angles from server.从服务器获得真实的角度。
|
|
||||||
res = func()
|
|
||||||
if res.joint_1 == res.joint_2 == res.joint_3 == 0.0:
|
|
||||||
continue
|
|
||||||
radians_list = [
|
|
||||||
res.joint_1 * (math.pi / 180),
|
|
||||||
res.joint_2 * (math.pi / 180),
|
|
||||||
res.joint_3 * (math.pi / 180),
|
|
||||||
res.joint_4 * (math.pi / 180),
|
|
||||||
res.joint_5 * (math.pi / 180),
|
|
||||||
res.joint_6 * (math.pi / 180),
|
|
||||||
]
|
|
||||||
rospy.loginfo("res: {}".format(radians_list))
|
|
||||||
|
|
||||||
# publish angles.发布角度
|
|
||||||
joint_state_send.header.stamp = rospy.Time.now()
|
|
||||||
joint_state_send.position = radians_list
|
|
||||||
pub.publish(joint_state_send)
|
|
||||||
rate.sleep()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
try:
|
|
||||||
talker()
|
|
||||||
except rospy.ROSInterruptException:
|
|
||||||
pass
|
|
||||||
|
|
@ -1,63 +0,0 @@
|
||||||
#!/usr/bin/env python2
|
|
||||||
import math
|
|
||||||
|
|
||||||
import rospy
|
|
||||||
from sensor_msgs.msg import JointState
|
|
||||||
from std_msgs.msg import Header
|
|
||||||
from mycobot_communication.msg import MycobotAngles
|
|
||||||
|
|
||||||
|
|
||||||
class Listener(object):
|
|
||||||
def __init__(self):
|
|
||||||
super(Listener, self).__init__()
|
|
||||||
|
|
||||||
rospy.loginfo("start ...")
|
|
||||||
rospy.init_node("real_listener_1", anonymous=True)
|
|
||||||
# init publisher.初始化发布者
|
|
||||||
self.pub = rospy.Publisher("joint_states", JointState, queue_size=10)
|
|
||||||
# init subscriber.初始化订阅者
|
|
||||||
self.sub = rospy.Subscriber("mycobot/angles_real", MycobotAngles, self.callback)
|
|
||||||
rospy.spin()
|
|
||||||
|
|
||||||
def callback(self, data):
|
|
||||||
"""`mycobot/angles_real` subscriber callback method.
|
|
||||||
|
|
||||||
Args:
|
|
||||||
data (MycobotAngles): callback argument.
|
|
||||||
"""
|
|
||||||
# ini publisher object. 初始化发布者对象
|
|
||||||
joint_state_send = JointState()
|
|
||||||
joint_state_send.header = Header()
|
|
||||||
|
|
||||||
joint_state_send.name = [
|
|
||||||
"joint2_to_joint1",
|
|
||||||
"joint3_to_joint2",
|
|
||||||
"joint4_to_joint3",
|
|
||||||
"joint5_to_joint4",
|
|
||||||
"joint6_to_joint5",
|
|
||||||
"joint6output_to_joint6",
|
|
||||||
]
|
|
||||||
joint_state_send.velocity = [0]
|
|
||||||
joint_state_send.effort = []
|
|
||||||
joint_state_send.header.stamp = rospy.Time.now()
|
|
||||||
|
|
||||||
# process callback data. 处理回调数据。
|
|
||||||
radians_list = [
|
|
||||||
data.joint_1 * (math.pi / 180),
|
|
||||||
data.joint_2 * (math.pi / 180),
|
|
||||||
data.joint_3 * (math.pi / 180),
|
|
||||||
data.joint_4 * (math.pi / 180),
|
|
||||||
data.joint_5 * (math.pi / 180),
|
|
||||||
data.joint_6 * (math.pi / 180),
|
|
||||||
]
|
|
||||||
rospy.loginfo("res: {}".format(radians_list))
|
|
||||||
|
|
||||||
joint_state_send.position = radians_list
|
|
||||||
self.pub.publish(joint_state_send)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
try:
|
|
||||||
Listener()
|
|
||||||
except rospy.ROSInterruptException:
|
|
||||||
pass
|
|
||||||
|
|
@ -1,478 +0,0 @@
|
||||||
#!/usr/bin/env python
|
|
||||||
# -*- coding: utf-8 -*-
|
|
||||||
import Tkinter as tk
|
|
||||||
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
|
||||||
import rospy
|
|
||||||
import time
|
|
||||||
from rospy import ServiceException
|
|
||||||
|
|
||||||
|
|
||||||
class Window:
|
|
||||||
def __init__(self, handle):
|
|
||||||
self.win = handle
|
|
||||||
self.win.resizable(0, 0) # fixed window size，固定窗口大小
|
|
||||||
|
|
||||||
self.model = 0
|
|
||||||
self.speed = rospy.get_param("~speed", 50)
|
|
||||||
|
|
||||||
# set default speed，设置默认速度
|
|
||||||
self.speed_d = tk.StringVar()
|
|
||||||
self.speed_d.set(str(self.speed))
|
|
||||||
# print(self.speed)
|
|
||||||
self.connect_ser()
|
|
||||||
|
|
||||||
# Get the data of the robotic arm，获取机械臂数据
|
|
||||||
self.record_coords = [0, 0, 0, 0, 0, 0, self.speed, self.model]
|
|
||||||
self.res_angles = [0, 0, 0, 0, 0, 0, self.speed, self.model]
|
|
||||||
self.get_date()
|
|
||||||
|
|
||||||
# get screen width and height.获取屏幕宽度和高度
|
|
||||||
self.ws = self.win.winfo_screenwidth() # width of the screen
|
|
||||||
self.hs = self.win.winfo_screenheight() # height of the screen
|
|
||||||
# calculate x and y coordinates for the Tk root window
|
|
||||||
# 计算 Tk 根窗口的 x 和 y 坐标
|
|
||||||
x = (self.ws / 2) - 190
|
|
||||||
y = (self.hs / 2) - 250
|
|
||||||
self.win.geometry("430x370+{}+{}".format(x, y))
|
|
||||||
# layout,布局
|
|
||||||
self.set_layout()
|
|
||||||
# input section,输入部分
|
|
||||||
self.need_input()
|
|
||||||
# Show part,展示部分
|
|
||||||
self.show_init()
|
|
||||||
|
|
||||||
# Set the joint buttons 设置joint按钮
|
|
||||||
tk.Button(self.frmLT, text="设置", width=5, command=self.get_joint_input).grid(
|
|
||||||
row=6, column=1, sticky="w", padx=3, pady=2
|
|
||||||
)
|
|
||||||
|
|
||||||
# coordination settings button,coordination 设置按钮
|
|
||||||
tk.Button(self.frmRT, text="设置", width=5, command=self.get_coord_input).grid(
|
|
||||||
row=6, column=1, sticky="w", padx=3, pady=2
|
|
||||||
)
|
|
||||||
|
|
||||||
# Gripper switch button,夹爪开关按钮
|
|
||||||
tk.Button(self.frmLB, text="夹爪(开)", command=self.gripper_open, width=5).grid(
|
|
||||||
row=1, column=0, sticky="w", padx=3, pady=50
|
|
||||||
)
|
|
||||||
tk.Button(self.frmLB, text="夹爪(关)", command=self.gripper_close, width=5).grid(
|
|
||||||
row=1, column=1, sticky="w", padx=3, pady=2
|
|
||||||
)
|
|
||||||
|
|
||||||
def connect_ser(self):
|
|
||||||
rospy.init_node("simple_gui", anonymous=True, disable_signals=True)
|
|
||||||
|
|
||||||
rospy.wait_for_service("get_joint_angles")
|
|
||||||
rospy.wait_for_service("set_joint_angles")
|
|
||||||
rospy.wait_for_service("get_joint_coords")
|
|
||||||
rospy.wait_for_service("set_joint_coords")
|
|
||||||
rospy.wait_for_service("switch_gripper_status")
|
|
||||||
try:
|
|
||||||
self.get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords)
|
|
||||||
self.set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords)
|
|
||||||
self.get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles)
|
|
||||||
self.set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles)
|
|
||||||
self.switch_gripper = rospy.ServiceProxy(
|
|
||||||
"switch_gripper_status", GripperStatus
|
|
||||||
)
|
|
||||||
except:
|
|
||||||
print("start error ...")
|
|
||||||
exit(1)
|
|
||||||
|
|
||||||
print("Connect service success.")
|
|
||||||
|
|
||||||
def set_layout(self):
|
|
||||||
self.frmLT = tk.Frame(width=200, height=200)
|
|
||||||
self.frmLC = tk.Frame(width=200, height=200)
|
|
||||||
self.frmLB = tk.Frame(width=200, height=200)
|
|
||||||
self.frmRT = tk.Frame(width=200, height=200)
|
|
||||||
self.frmLT.grid(row=0, column=0, padx=1, pady=3)
|
|
||||||
self.frmLC.grid(row=1, column=0, padx=1, pady=3)
|
|
||||||
self.frmLB.grid(row=1, column=1, padx=2, pady=3)
|
|
||||||
self.frmRT.grid(row=0, column=1, padx=2, pady=3)
|
|
||||||
|
|
||||||
def need_input(self):
|
|
||||||
# input hint,输入提示
|
|
||||||
tk.Label(self.frmLT, text="Joint 1 ").grid(row=0)
|
|
||||||
tk.Label(self.frmLT, text="Joint 2 ").grid(row=1) # the second row,第二行
|
|
||||||
tk.Label(self.frmLT, text="Joint 3 ").grid(row=2)
|
|
||||||
tk.Label(self.frmLT, text="Joint 4 ").grid(row=3)
|
|
||||||
tk.Label(self.frmLT, text="Joint 5 ").grid(row=4)
|
|
||||||
tk.Label(self.frmLT, text="Joint 6 ").grid(row=5)
|
|
||||||
|
|
||||||
tk.Label(self.frmRT, text=" x ").grid(row=0)
|
|
||||||
tk.Label(self.frmRT, text=" y ").grid(row=1) # the second row,第二行
|
|
||||||
tk.Label(self.frmRT, text=" z ").grid(row=2)
|
|
||||||
tk.Label(self.frmRT, text=" rx ").grid(row=3)
|
|
||||||
tk.Label(self.frmRT, text=" ry ").grid(row=4)
|
|
||||||
tk.Label(self.frmRT, text=" rz ").grid(row=5)
|
|
||||||
|
|
||||||
# Set the default value of the input box,设置输入框的默认值
|
|
||||||
self.j1_default = tk.StringVar()
|
|
||||||
self.j1_default.set(self.res_angles[0])
|
|
||||||
self.j2_default = tk.StringVar()
|
|
||||||
self.j2_default.set(self.res_angles[1])
|
|
||||||
self.j3_default = tk.StringVar()
|
|
||||||
self.j3_default.set(self.res_angles[2])
|
|
||||||
self.j4_default = tk.StringVar()
|
|
||||||
self.j4_default.set(self.res_angles[3])
|
|
||||||
self.j5_default = tk.StringVar()
|
|
||||||
self.j5_default.set(self.res_angles[4])
|
|
||||||
self.j6_default = tk.StringVar()
|
|
||||||
self.j6_default.set(self.res_angles[5])
|
|
||||||
|
|
||||||
self.x_default = tk.StringVar()
|
|
||||||
self.x_default.set(self.record_coords[0])
|
|
||||||
self.y_default = tk.StringVar()
|
|
||||||
self.y_default.set(self.record_coords[1])
|
|
||||||
self.z_default = tk.StringVar()
|
|
||||||
self.z_default.set(self.record_coords[2])
|
|
||||||
self.rx_default = tk.StringVar()
|
|
||||||
self.rx_default.set(self.record_coords[3])
|
|
||||||
self.ry_default = tk.StringVar()
|
|
||||||
self.ry_default.set(self.record_coords[4])
|
|
||||||
self.rz_default = tk.StringVar()
|
|
||||||
self.rz_default.set(self.record_coords[5])
|
|
||||||
|
|
||||||
# joint input box,joint 输入框
|
|
||||||
self.J_1 = tk.Entry(self.frmLT, textvariable=self.j1_default)
|
|
||||||
self.J_1.grid(row=0, column=1, pady=3)
|
|
||||||
self.J_2 = tk.Entry(self.frmLT, textvariable=self.j2_default)
|
|
||||||
self.J_2.grid(row=1, column=1, pady=3)
|
|
||||||
self.J_3 = tk.Entry(self.frmLT, textvariable=self.j3_default)
|
|
||||||
self.J_3.grid(row=2, column=1, pady=3)
|
|
||||||
self.J_4 = tk.Entry(self.frmLT, textvariable=self.j4_default)
|
|
||||||
self.J_4.grid(row=3, column=1, pady=3)
|
|
||||||
self.J_5 = tk.Entry(self.frmLT, textvariable=self.j5_default)
|
|
||||||
self.J_5.grid(row=4, column=1, pady=3)
|
|
||||||
self.J_6 = tk.Entry(self.frmLT, textvariable=self.j6_default)
|
|
||||||
self.J_6.grid(row=5, column=1, pady=3)
|
|
||||||
|
|
||||||
# coord input box,coord 输入框
|
|
||||||
self.x = tk.Entry(self.frmRT, textvariable=self.x_default)
|
|
||||||
self.x.grid(row=0, column=1, pady=3, padx=0)
|
|
||||||
self.y = tk.Entry(self.frmRT, textvariable=self.y_default)
|
|
||||||
self.y.grid(row=1, column=1, pady=3)
|
|
||||||
self.z = tk.Entry(self.frmRT, textvariable=self.z_default)
|
|
||||||
self.z.grid(row=2, column=1, pady=3)
|
|
||||||
self.rx = tk.Entry(self.frmRT, textvariable=self.rx_default)
|
|
||||||
self.rx.grid(row=3, column=1, pady=3)
|
|
||||||
self.ry = tk.Entry(self.frmRT, textvariable=self.ry_default)
|
|
||||||
self.ry.grid(row=4, column=1, pady=3)
|
|
||||||
self.rz = tk.Entry(self.frmRT, textvariable=self.rz_default)
|
|
||||||
self.rz.grid(row=5, column=1, pady=3)
|
|
||||||
|
|
||||||
# All input boxes, used to get the input data,所有输入框，用于拿输入的数据
|
|
||||||
self.all_j = [self.J_1, self.J_2, self.J_3, self.J_4, self.J_5, self.J_6]
|
|
||||||
self.all_c = [self.x, self.y, self.z, self.rx, self.ry, self.rz]
|
|
||||||
|
|
||||||
# speed input box,速度输入框
|
|
||||||
tk.Label(
|
|
||||||
self.frmLB,
|
|
||||||
text="speed",
|
|
||||||
).grid(row=0, column=0)
|
|
||||||
self.get_speed = tk.Entry(self.frmLB, textvariable=self.speed_d, width=10)
|
|
||||||
self.get_speed.grid(row=0, column=1)
|
|
||||||
|
|
||||||
def show_init(self):
|
|
||||||
# show,显示
|
|
||||||
tk.Label(self.frmLC, text="Joint 1 ").grid(row=0)
|
|
||||||
tk.Label(self.frmLC, text="Joint 2 ").grid(row=1) # the second row,第二行
|
|
||||||
tk.Label(self.frmLC, text="Joint 3 ").grid(row=2)
|
|
||||||
tk.Label(self.frmLC, text="Joint 4 ").grid(row=3)
|
|
||||||
tk.Label(self.frmLC, text="Joint 5 ").grid(row=4)
|
|
||||||
tk.Label(self.frmLC, text="Joint 6 ").grid(row=5)
|
|
||||||
|
|
||||||
# get数据
|
|
||||||
|
|
||||||
# show,展示出来
|
|
||||||
self.cont_1 = tk.StringVar(self.frmLC)
|
|
||||||
self.cont_1.set(str(self.res_angles[0]) + "°")
|
|
||||||
self.cont_2 = tk.StringVar(self.frmLC)
|
|
||||||
self.cont_2.set(str(self.res_angles[1]) + "°")
|
|
||||||
self.cont_3 = tk.StringVar(self.frmLC)
|
|
||||||
self.cont_3.set(str(self.res_angles[2]) + "°")
|
|
||||||
self.cont_4 = tk.StringVar(self.frmLC)
|
|
||||||
self.cont_4.set(str(self.res_angles[3]) + "°")
|
|
||||||
self.cont_5 = tk.StringVar(self.frmLC)
|
|
||||||
self.cont_5.set(str(self.res_angles[4]) + "°")
|
|
||||||
self.cont_6 = tk.StringVar(self.frmLC)
|
|
||||||
self.cont_6.set(str(self.res_angles[5]) + "°")
|
|
||||||
self.cont_all = [
|
|
||||||
self.cont_1,
|
|
||||||
self.cont_2,
|
|
||||||
self.cont_3,
|
|
||||||
self.cont_4,
|
|
||||||
self.cont_5,
|
|
||||||
self.cont_6,
|
|
||||||
self.speed,
|
|
||||||
self.model,
|
|
||||||
]
|
|
||||||
|
|
||||||
self.show_j1 = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.cont_1,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=0, column=1, padx=0, pady=5)
|
|
||||||
|
|
||||||
self.show_j2 = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.cont_2,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=1, column=1, padx=0, pady=5)
|
|
||||||
self.show_j3 = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.cont_3,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=2, column=1, padx=0, pady=5)
|
|
||||||
self.show_j4 = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.cont_4,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=3, column=1, padx=0, pady=5)
|
|
||||||
self.show_j5 = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.cont_5,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=4, column=1, padx=0, pady=5)
|
|
||||||
self.show_j6 = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.cont_6,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=5, column=1, padx=5, pady=5)
|
|
||||||
|
|
||||||
self.all_jo = [
|
|
||||||
self.show_j1,
|
|
||||||
self.show_j2,
|
|
||||||
self.show_j3,
|
|
||||||
self.show_j4,
|
|
||||||
self.show_j5,
|
|
||||||
self.show_j6,
|
|
||||||
]
|
|
||||||
|
|
||||||
# show,显示
|
|
||||||
tk.Label(self.frmLC, text=" x ").grid(row=0, column=3)
|
|
||||||
tk.Label(self.frmLC, text=" y ").grid(row=1, column=3)
|
|
||||||
tk.Label(self.frmLC, text=" z ").grid(row=2, column=3)
|
|
||||||
tk.Label(self.frmLC, text=" rx ").grid(row=3, column=3)
|
|
||||||
tk.Label(self.frmLC, text=" ry ").grid(row=4, column=3)
|
|
||||||
tk.Label(self.frmLC, text=" rz ").grid(row=5, column=3)
|
|
||||||
self.coord_x = tk.StringVar()
|
|
||||||
self.coord_x.set(str(self.record_coords[0]))
|
|
||||||
self.coord_y = tk.StringVar()
|
|
||||||
self.coord_y.set(str(self.record_coords[1]))
|
|
||||||
self.coord_z = tk.StringVar()
|
|
||||||
self.coord_z.set(str(self.record_coords[2]))
|
|
||||||
self.coord_rx = tk.StringVar()
|
|
||||||
self.coord_rx.set(str(self.record_coords[3]))
|
|
||||||
self.coord_ry = tk.StringVar()
|
|
||||||
self.coord_ry.set(str(self.record_coords[4]))
|
|
||||||
self.coord_rz = tk.StringVar()
|
|
||||||
self.coord_rz.set(str(self.record_coords[5]))
|
|
||||||
|
|
||||||
self.coord_all = [
|
|
||||||
self.coord_x,
|
|
||||||
self.coord_y,
|
|
||||||
self.coord_z,
|
|
||||||
self.coord_rx,
|
|
||||||
self.coord_ry,
|
|
||||||
self.coord_rz,
|
|
||||||
self.speed,
|
|
||||||
self.model,
|
|
||||||
]
|
|
||||||
|
|
||||||
self.show_x = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.coord_x,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=0, column=4, padx=5, pady=5)
|
|
||||||
self.show_y = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.coord_y,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=1, column=4, padx=5, pady=5)
|
|
||||||
self.show_z = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.coord_z,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=2, column=4, padx=5, pady=5)
|
|
||||||
self.show_rx = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.coord_rx,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=3, column=4, padx=5, pady=5)
|
|
||||||
self.show_ry = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.coord_ry,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=4, column=4, padx=5, pady=5)
|
|
||||||
self.show_rz = tk.Label(
|
|
||||||
self.frmLC,
|
|
||||||
textvariable=self.coord_rz,
|
|
||||||
font=("Arial", 9),
|
|
||||||
width=7,
|
|
||||||
height=1,
|
|
||||||
bg="white",
|
|
||||||
).grid(row=5, column=4, padx=5, pady=5)
|
|
||||||
|
|
||||||
# mm， Unit show，单位展示
|
|
||||||
self.unit = tk.StringVar()
|
|
||||||
self.unit.set("mm")
|
|
||||||
for i in range(6):
|
|
||||||
tk.Label(self.frmLC, textvariable=self.unit, font=("Arial", 9)).grid(
|
|
||||||
row=i, column=5
|
|
||||||
)
|
|
||||||
|
|
||||||
def gripper_open(self):
|
|
||||||
try:
|
|
||||||
self.switch_gripper(True)
|
|
||||||
except ServiceException:
|
|
||||||
# Probably because the method has no return value, the service throws an unhandled error
|
|
||||||
# 可能由于该方法没有返回值，服务抛出无法处理的错误
|
|
||||||
pass
|
|
||||||
|
|
||||||
def gripper_close(self):
|
|
||||||
try:
|
|
||||||
self.switch_gripper(False)
|
|
||||||
except ServiceException:
|
|
||||||
pass
|
|
||||||
|
|
||||||
def get_coord_input(self):
|
|
||||||
# Get the data input by coord and send it to the robotic arm
|
|
||||||
# 获取 coord 输入的数据，发送给机械臂
|
|
||||||
c_value = []
|
|
||||||
for i in self.all_c:
|
|
||||||
# print(type(i.get()))
|
|
||||||
c_value.append(float(i.get()))
|
|
||||||
self.speed = (
|
|
||||||
int(float(self.get_speed.get())) if self.get_speed.get() else self.speed
|
|
||||||
)
|
|
||||||
c_value.append(self.speed)
|
|
||||||
c_value.append(self.model)
|
|
||||||
# print(c_value)
|
|
||||||
try:
|
|
||||||
self.set_coords(*c_value)
|
|
||||||
except ServiceException:
|
|
||||||
pass
|
|
||||||
self.show_j_date(c_value[:-2], "coord")
|
|
||||||
|
|
||||||
def get_joint_input(self):
|
|
||||||
# Get the data input by the joint and send it to the robotic arm
|
|
||||||
# 获取joint输入的数据，发送给机械臂
|
|
||||||
j_value = []
|
|
||||||
for i in self.all_j:
|
|
||||||
# print(type(i.get()))
|
|
||||||
j_value.append(float(i.get()))
|
|
||||||
self.speed = (
|
|
||||||
int(float(self.get_speed.get())) if self.get_speed.get() else self.speed
|
|
||||||
)
|
|
||||||
j_value.append(self.speed)
|
|
||||||
|
|
||||||
try:
|
|
||||||
self.set_angles(*j_value)
|
|
||||||
except ServiceException:
|
|
||||||
pass
|
|
||||||
self.show_j_date(j_value[:-1])
|
|
||||||
# return j_value,c_value,speed
|
|
||||||
|
|
||||||
def get_date(self):
|
|
||||||
# Take the data of the robotic arm for display.拿机械臂的数据，用于展示
|
|
||||||
t = time.time()
|
|
||||||
while time.time() - t < 2:
|
|
||||||
self.res = self.get_coords()
|
|
||||||
if self.res.x > 1:
|
|
||||||
break
|
|
||||||
time.sleep(0.1)
|
|
||||||
|
|
||||||
t = time.time()
|
|
||||||
while time.time() - t < 2:
|
|
||||||
self.angles = self.get_angles()
|
|
||||||
if self.angles.joint_1 > 1:
|
|
||||||
break
|
|
||||||
time.sleep(0.1)
|
|
||||||
# print(self.angles.joint_1)
|
|
||||||
self.record_coords = [
|
|
||||||
round(self.res.x, 2),
|
|
||||||
round(self.res.y, 2),
|
|
||||||
round(self.res.z, 2),
|
|
||||||
round(self.res.rx, 2),
|
|
||||||
round(self.res.ry, 2),
|
|
||||||
round(self.res.rz, 2),
|
|
||||||
self.speed,
|
|
||||||
self.model,
|
|
||||||
]
|
|
||||||
self.res_angles = [
|
|
||||||
round(self.angles.joint_1, 2),
|
|
||||||
round(self.angles.joint_2, 2),
|
|
||||||
round(self.angles.joint_3, 2),
|
|
||||||
round(self.angles.joint_4, 2),
|
|
||||||
round(self.angles.joint_5, 2),
|
|
||||||
round(self.angles.joint_6, 2),
|
|
||||||
]
|
|
||||||
# print('coord:',self.record_coords)
|
|
||||||
# print('angles:',self.res_angles)
|
|
||||||
|
|
||||||
# def send_input(self,dates):
|
|
||||||
def show_j_date(self, date, way=""):
|
|
||||||
# Show data,展示数据
|
|
||||||
if way == "coord":
|
|
||||||
for i, j in zip(date, self.coord_all):
|
|
||||||
# print(i)
|
|
||||||
j.set(str(i))
|
|
||||||
else:
|
|
||||||
for i, j in zip(date, self.cont_all):
|
|
||||||
j.set(str(i) + "°")
|
|
||||||
|
|
||||||
def run(self):
|
|
||||||
while True:
|
|
||||||
try:
|
|
||||||
self.win.update()
|
|
||||||
time.sleep(0.001)
|
|
||||||
except tk.TclError as e:
|
|
||||||
if "application has been destroyed" in str(e):
|
|
||||||
break
|
|
||||||
else:
|
|
||||||
raise
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
window = tk.Tk()
|
|
||||||
window.title("mycobot ros GUI")
|
|
||||||
Window(window).run()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
|
|
@ -1,49 +0,0 @@
|
||||||
#!/usr/bin/env python2
|
|
||||||
|
|
||||||
"""[summary]
|
|
||||||
This file obtains the joint angle of the manipulator in ROS,
|
|
||||||
and then sends it directly to the real manipulator using `pymycobot` API.
|
|
||||||
This file is [slider_control.launch] related script.
|
|
||||||
Passable parameters:
|
|
||||||
port: serial prot string. Defaults is '/dev/ttyTHS1'
|
|
||||||
baud: serial prot baudrate. Defaults is 1000000.
|
|
||||||
"""
|
|
||||||
|
|
||||||
import rospy
|
|
||||||
from sensor_msgs.msg import JointState
|
|
||||||
|
|
||||||
from pymycobot.mycobot import MyCobot
|
|
||||||
|
|
||||||
|
|
||||||
mc = None
|
|
||||||
|
|
||||||
|
|
||||||
def callback(data):
|
|
||||||
# rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
|
|
||||||
print(data.position)
|
|
||||||
data_list = []
|
|
||||||
for index, value in enumerate(data.position):
|
|
||||||
data_list.append(value)
|
|
||||||
|
|
||||||
mc.send_radians(data_list, 80)
|
|
||||||
# time.sleep(0.5)
|
|
||||||
|
|
||||||
|
|
||||||
def listener():
|
|
||||||
global mc
|
|
||||||
rospy.init_node("control_slider", anonymous=True)
|
|
||||||
|
|
||||||
rospy.Subscriber("joint_states", JointState, callback)
|
|
||||||
port = rospy.get_param("~port", "/dev/ttyTHS1")
|
|
||||||
baud = rospy.get_param("~baud", 1000000)
|
|
||||||
print(port, baud)
|
|
||||||
mc = MyCobot(port, baud)
|
|
||||||
|
|
||||||
# spin() simply keeps python from exiting until this node is stopped
|
|
||||||
# spin()只是阻止python退出，直到该节点停止
|
|
||||||
print("spin ...")
|
|
||||||
rospy.spin()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
listener()
|
|
||||||
|
|
@ -1,174 +0,0 @@
|
||||||
#!/usr/bin/env python
|
|
||||||
from __future__ import print_function
|
|
||||||
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
|
||||||
import rospy
|
|
||||||
import sys
|
|
||||||
import select
|
|
||||||
import termios
|
|
||||||
import tty
|
|
||||||
import time
|
|
||||||
|
|
||||||
import roslib
|
|
||||||
|
|
||||||
# Terminal output prompt information. 终端输出提示信息
|
|
||||||
msg = """\
|
|
||||||
Mycobot Teleop Keyboard Controller
|
|
||||||
---------------------------
|
|
||||||
Movimg options(control coordinations [x,y,z,rx,ry,rz]):
|
|
||||||
w(x+)
|
|
||||||
|
|
||||||
a(y-) s(x-) d(y+)
|
|
||||||
|
|
||||||
z(z-) x(z+)
|
|
||||||
|
|
||||||
u(rx+) i(ry+) o(rz+)
|
|
||||||
j(rx-) k(ry-) l(rz-)
|
|
||||||
|
|
||||||
Gripper control:
|
|
||||||
g - open
|
|
||||||
h - close
|
|
||||||
|
|
||||||
Other:
|
|
||||||
1 - Go to init pose
|
|
||||||
2 - Go to home pose
|
|
||||||
3 - Resave home pose
|
|
||||||
q - Quit
|
|
||||||
"""
|
|
||||||
|
|
||||||
|
|
||||||
def vels(speed, turn):
|
|
||||||
return "currently:\tspeed: %s\tchange percent: %s " % (speed, turn)
|
|
||||||
|
|
||||||
|
|
||||||
class Raw(object):
|
|
||||||
def __init__(self, stream):
|
|
||||||
self.stream = stream
|
|
||||||
self.fd = self.stream.fileno()
|
|
||||||
|
|
||||||
def __enter__(self):
|
|
||||||
self.original_stty = termios.tcgetattr(self.stream)
|
|
||||||
tty.setcbreak(self.stream)
|
|
||||||
|
|
||||||
def __exit__(self, type, value, traceback):
|
|
||||||
termios.tcsetattr(self.stream, termios.TCSANOW, self.original_stty)
|
|
||||||
|
|
||||||
|
|
||||||
def teleop_keyboard():
|
|
||||||
rospy.init_node("teleop_keyboard")
|
|
||||||
|
|
||||||
model = 0
|
|
||||||
speed = rospy.get_param("~speed", 50)
|
|
||||||
change_percent = rospy.get_param("~change_percent", 5)
|
|
||||||
|
|
||||||
change_angle = 180 * change_percent / 100
|
|
||||||
change_len = 250 * change_percent / 100
|
|
||||||
|
|
||||||
rospy.wait_for_service("get_joint_angles")
|
|
||||||
rospy.wait_for_service("set_joint_angles")
|
|
||||||
rospy.wait_for_service("get_joint_coords")
|
|
||||||
rospy.wait_for_service("set_joint_coords")
|
|
||||||
rospy.wait_for_service("switch_gripper_status")
|
|
||||||
print("service ready.")
|
|
||||||
try:
|
|
||||||
get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords)
|
|
||||||
set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords)
|
|
||||||
get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles)
|
|
||||||
set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles)
|
|
||||||
switch_gripper = rospy.ServiceProxy(
|
|
||||||
"switch_gripper_status", GripperStatus)
|
|
||||||
except:
|
|
||||||
print("start error ...")
|
|
||||||
exit(1)
|
|
||||||
|
|
||||||
init_pose = [0, 0, 0, 0, 0, 0, speed]
|
|
||||||
home_pose = [0, 8, -127, 40, 0, 0, speed]
|
|
||||||
|
|
||||||
# rsp = set_angles(*init_pose)
|
|
||||||
|
|
||||||
while True:
|
|
||||||
res = get_coords()
|
|
||||||
if res.x > 1:
|
|
||||||
break
|
|
||||||
time.sleep(0.1)
|
|
||||||
|
|
||||||
record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model]
|
|
||||||
print(record_coords)
|
|
||||||
|
|
||||||
try:
|
|
||||||
print(msg)
|
|
||||||
print(vels(speed, change_percent))
|
|
||||||
# Keyboard keys call different motion functions. 键盘按键调用不同的运动功能
|
|
||||||
while 1:
|
|
||||||
try:
|
|
||||||
# print("\r current coords: %s" % record_coords, end="")
|
|
||||||
with Raw(sys.stdin):
|
|
||||||
key = sys.stdin.read(1)
|
|
||||||
if key == "q":
|
|
||||||
break
|
|
||||||
elif key in ["w", "W"]:
|
|
||||||
record_coords[0] += change_len
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["s", "S"]:
|
|
||||||
record_coords[0] -= change_len
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["a", "A"]:
|
|
||||||
record_coords[1] -= change_len
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["d", "D"]:
|
|
||||||
record_coords[1] += change_len
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["z", "Z"]:
|
|
||||||
record_coords[2] -= change_len
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["x", "X"]:
|
|
||||||
record_coords[2] += change_len
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["u", "U"]:
|
|
||||||
record_coords[3] += change_angle
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["j", "J"]:
|
|
||||||
record_coords[3] -= change_angle
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["i", "I"]:
|
|
||||||
record_coords[4] += change_angle
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["k", "K"]:
|
|
||||||
record_coords[4] -= change_angle
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["o", "O"]:
|
|
||||||
record_coords[5] += change_angle
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["l", "L"]:
|
|
||||||
record_coords[5] -= change_angle
|
|
||||||
set_coords(*record_coords)
|
|
||||||
elif key in ["g", "G"]:
|
|
||||||
switch_gripper(True)
|
|
||||||
elif key in ["h", "H"]:
|
|
||||||
switch_gripper(False)
|
|
||||||
elif key == "1":
|
|
||||||
rsp = set_angles(*init_pose)
|
|
||||||
elif key in "2":
|
|
||||||
rsp = set_angles(*home_pose)
|
|
||||||
elif key in "3":
|
|
||||||
rep = get_angles()
|
|
||||||
home_pose[0] = rep.joint_1
|
|
||||||
home_pose[1] = rep.joint_2
|
|
||||||
home_pose[2] = rep.joint_3
|
|
||||||
home_pose[3] = rep.joint_4
|
|
||||||
home_pose[5] = rep.joint_5
|
|
||||||
else:
|
|
||||||
continue
|
|
||||||
|
|
||||||
except Exception as e:
|
|
||||||
# print(e)
|
|
||||||
continue
|
|
||||||
|
|
||||||
except Exception as e:
|
|
||||||
print(e)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
try:
|
|
||||||
teleop_keyboard()
|
|
||||||
except rospy.ROSInterruptException:
|
|
||||||
pass
|
|
||||||
|
|
@ -1,29 +0,0 @@
|
||||||
#include <ros/ros.h>
|
|
||||||
#include <image_transport/image_transport.h>
|
|
||||||
#include <opencv2/highgui/highgui.hpp>
|
|
||||||
#include <cv_bridge/cv_bridge.h>
|
|
||||||
|
|
||||||
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
|
|
||||||
{
|
|
||||||
try
|
|
||||||
{
|
|
||||||
cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
|
|
||||||
cv::waitKey(30);
|
|
||||||
}
|
|
||||||
catch (cv_bridge::Exception &e)
|
|
||||||
{
|
|
||||||
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(int argc, char **argv)
|
|
||||||
{
|
|
||||||
ros::init(argc, argv, "image_listener");
|
|
||||||
ros::NodeHandle nh;
|
|
||||||
cv::namedWindow("view");
|
|
||||||
cv::startWindowThread();
|
|
||||||
image_transport::ImageTransport it(nh);
|
|
||||||
image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback);
|
|
||||||
ros::spin();
|
|
||||||
cv::destroyWindow("view");
|
|
||||||
}
|
|
||||||
|
|
@ -1,60 +0,0 @@
|
||||||
#include <ros/ros.h>
|
|
||||||
#include <image_transport/image_transport.h>
|
|
||||||
#include <opencv2/highgui/highgui.hpp>
|
|
||||||
#include <cv_bridge/cv_bridge.h>
|
|
||||||
#include <sstream> // for converting the command line parameter to integer,用于将命令行参数转换为整数
|
|
||||||
|
|
||||||
int main(int argc, char **argv)
|
|
||||||
{
|
|
||||||
// Check if video source has been passed as a parameter,检查视频源是否已作为参数传递
|
|
||||||
if (argv[1] == NULL)
|
|
||||||
{
|
|
||||||
ROS_INFO("argv[1]=NULL\n");
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
ros::init(argc, argv, "image_publisher"); // Initialize node,初始化节点
|
|
||||||
ros::NodeHandle nh;
|
|
||||||
image_transport::ImageTransport it(nh);
|
|
||||||
image_transport::Publisher pub = it.advertise("camera/image", 1); // Publish topic，发布话题
|
|
||||||
|
|
||||||
ros::Rate loop_rate(200); // refresh Hz.
|
|
||||||
|
|
||||||
// Convert the passed as command line parameter index for the video device to an integer，
|
|
||||||
// 将作为命令行参数传递的视频设备索引转换为整数
|
|
||||||
std::istringstream video_sourceCmd(argv[1]);
|
|
||||||
int video_source;
|
|
||||||
// Check if it is indeed a number，检查它是否确实是一个数字
|
|
||||||
if (!(video_sourceCmd >> video_source))
|
|
||||||
{
|
|
||||||
ROS_INFO("video_sourceCmd is %d\n", video_source);
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
cv::VideoCapture cap(video_source);
|
|
||||||
// Check if video device can be opened with the given index，检查是否可以使用给定的索引打开视频设备
|
|
||||||
if (!cap.isOpened())
|
|
||||||
{
|
|
||||||
ROS_INFO("can not opencv video device\n");
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
cv::Mat frame;
|
|
||||||
sensor_msgs::ImagePtr msg;
|
|
||||||
|
|
||||||
while (nh.ok())
|
|
||||||
{
|
|
||||||
cap >> frame;
|
|
||||||
// cv::imshow("veiwer", frame);
|
|
||||||
// Check if grabbed frame is actually full with some content，检查抓取的帧是否实际上充满了一些内容
|
|
||||||
if (!frame.empty())
|
|
||||||
{
|
|
||||||
msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
|
|
||||||
pub.publish(msg);
|
|
||||||
//cv::Wait(1);
|
|
||||||
}
|
|
||||||
ros::spinOnce();
|
|
||||||
loop_rate.sleep();
|
|
||||||
// if(cv::waitKey(2) >= 0)
|
|
||||||
// break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
Loading…
Add table
Reference in a new issue