mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 01:50:46 +02:00
Moved everything to rostests
This commit is contained in:
@@ -1,40 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import rospy
|
||||
from ur_msgs.srv import SetIO
|
||||
from ur_msgs.msg import IOStates
|
||||
|
||||
def main():
|
||||
rospy.init_node('io_testing_client')
|
||||
service_client = rospy.ServiceProxy('/ur_hardware_interface/set_io', SetIO)
|
||||
service_client.wait_for_service()
|
||||
|
||||
maximum_messages = 5
|
||||
pin = 0
|
||||
|
||||
service_client(1, pin, 0)
|
||||
messages = 0
|
||||
pin_state = True
|
||||
|
||||
while(pin_state):
|
||||
if messages >= 5:
|
||||
return False
|
||||
io_state = rospy.wait_for_message('/ur_hardware_interface/io_states', IOStates)
|
||||
pin_state = io_state.digital_out_states[pin].state
|
||||
messages += 1
|
||||
|
||||
service_client(1, pin, 1)
|
||||
messages = 0
|
||||
pin_state = False
|
||||
|
||||
while(not pin_state):
|
||||
if messages >= 5:
|
||||
return False
|
||||
io_state = rospy.wait_for_message('/ur_hardware_interface/io_states', IOStates)
|
||||
pin_state = io_state.digital_out_states[pin].state
|
||||
messages += 1
|
||||
|
||||
return True
|
||||
|
||||
if __name__ == '__main__':
|
||||
print(main())
|
||||
@@ -1,37 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import rospy
|
||||
import actionlib
|
||||
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
|
||||
from trajectory_msgs.msg import JointTrajectoryPoint
|
||||
|
||||
def main(position_list, duration_list):
|
||||
rospy.init_node('trajectory_testing_client')
|
||||
client = actionlib.SimpleActionClient('/scaled_pos_traj_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
|
||||
client.wait_for_server()
|
||||
goal = FollowJointTrajectoryGoal()
|
||||
|
||||
goal.trajectory.joint_names = ["elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint", "wrist_1_joint",
|
||||
"wrist_2_joint", "wrist_3_joint"]
|
||||
|
||||
for i in range(len(position_list)):
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = position_list[i]
|
||||
point.time_from_start = rospy.Duration(duration_list[i])
|
||||
goal.trajectory.points.append(point)
|
||||
|
||||
client.send_goal(goal)
|
||||
client.wait_for_result()
|
||||
|
||||
return client.get_result().error_code == 0
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
position_list = [[0.0 for i in range(6)]]
|
||||
position_list.append([-0.5 for i in range(6)])
|
||||
position_list.append([-1.0 for i in range(6)])
|
||||
duration_list = [6.0, 9.0, 12.0]
|
||||
|
||||
print(main(position_list, duration_list))
|
||||
Reference in New Issue
Block a user