-
Notifications
You must be signed in to change notification settings - Fork 83
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
14 changed files
with
1,013 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
20 changes: 20 additions & 0 deletions
20
mycobot_280/mycobot_280jn/launch/slider_control_parallel_gripper.launch
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,20 @@ | ||
<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/mycobot_280_jn/mycobot_280_jn_parallel_gripper.urdf"/> | ||
<arg name="rvizconfig" default="$(find mycobot_280jn)/config/mycobot_jn.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> | ||
|
||
<!-- Show in Rviz .显示在Rviz--> | ||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" /> | ||
</launch> |
18 changes: 18 additions & 0 deletions
18
mycobot_280/mycobot_280jn/launch/test_parallel_gripper.launch
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,18 @@ | ||
<launch> | ||
<!-- Load URDF, rviz, etc. on the parameter server,加载参数服务器上的URDF、rviz等 --> | ||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot_280_jn/mycobot_280_jn_parallel_gripper.urdf"/> | ||
<!-- <arg name="model" default="$(find mycobot_description)/urdf/parallel_gripper/mycobot_parallel_gripper.urdf"/> --> | ||
<arg name="rvizconfig" default="$(find mycobot_280jn)/config/mycobot_jn.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> | ||
<!-- Show in Rviz ,显示在Rviz--> | ||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" /> | ||
</launch> |
58 changes: 58 additions & 0 deletions
58
mycobot_280/mycobot_280jn/scripts/slider_control_parallel_gripper.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,58 @@ | ||
#!/usr/bin/env python3 | ||
# encoding:utf-8 | ||
"""[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_parallel_gripper.launch] related script. | ||
Passable parameters: | ||
port: serial prot string. Defaults is '/dev/ttyTHS1' | ||
baud: serial prot baudrate. Defaults is 1000000. | ||
""" | ||
import time | ||
import math | ||
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) | ||
data_list = [] | ||
for index, value in enumerate(data.position): | ||
data_list.append(round(value,3)) | ||
# print(data_list[6:]) | ||
|
||
data_list = data_list[:7] | ||
print("radians:%s"%data_list[:6]) | ||
mc.send_radians(data_list[:6], 25) | ||
# 线性插值公式 gripper_value = (current_value - min_value) / (max_value - min_value) * 100 | ||
gripper_value = int((data_list[6] - (-0.007)) / (0 - (-0.007)) * 100) | ||
print("gripper_value:%s"%gripper_value) | ||
mc.set_gripper_value(gripper_value, 80, 3) | ||
|
||
|
||
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) | ||
time.sleep(0.05) | ||
mc.set_fresh_mode(1) | ||
time.sleep(0.05) | ||
|
||
# spin() simply keeps python from exiting until this node is stopped | ||
# spin()只是阻止python退出,直到该节点停止 | ||
print("spin ...") | ||
rospy.spin() | ||
|
||
|
||
if __name__ == "__main__": | ||
listener() |
Oops, something went wrong.