Commit e5f26ae9 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

sending Trajectory

parent 9c703498
......@@ -5,6 +5,8 @@ from trajectory_msgs.msg import (
)
from sensor_msgs.msg import JointState
import time
from copy import copy, deepcopy
class controller:
......@@ -13,17 +15,17 @@ class controller:
Initiate connection to UR5
"""
self.node = rospy.init_node("py_controller", anonymous = False)
self.traj = rospy.Publisher('scaled_pos_traj_controller/command', JointTrajectory, queue_size=10)
self.traj = rospy.Publisher("scaled_pos_traj_controller/command", JointTrajectory, queue_size=1)
self.joint_state = None
time.sleep(1)
def exec_traj(self, traj):
"""
execute a JointTrajectory object
"""
rospy.loginfo("sending trajectroy to UR5")
pub.publish(traj)
self.traj.publish(traj)
def get_joints(self):
......@@ -31,20 +33,52 @@ class controller:
Get current state of UR5
"""
def joint_state_setter(data):
self.joint_state = data.position
self.joint_state = data
self.sb.unregister()
self.sb = rospy.Subscriber("joint_states", JointState, joint_state_setter)
time.sleep(0.1)
return self.joint_state
return self.joint_state.position
def empty_traj(self):
"""
Empty trajectory with Joint names filled in
"""
t = JointTrajectory()
t.joint_names = self.joint_state.name
return t
if __name__=="__main__":
print "Testing joint angle"
########################################
print "Testing joint angle subscriber"
con = controller()
print con.get_joints()
########################################
print "Testing Joint Trajectory controller"
t = con.empty_traj()
js1 = (-1.3825197219848633,-1.7300764522948207,1.800080776214599,-1.4899586004069825,1.5598526000976562,0.7699174880981445)
js2 = (-1.3825197219848633,-1.7300764522948207,2.323414109547933,-1.4899586004069825,1.5598526000976562,0.7699174880981445)
p1 = JointTrajectoryPoint()
p1.positions = copy(js1)
p1.time_from_start = rospy.Time(0)
p2 = JointTrajectoryPoint()
p2.positions = copy(js2)
p2.time_from_start = rospy.Time(3)
p3 = JointTrajectoryPoint()
p3.positions = copy(js1)
p3.time_from_start = rospy.Time(6)
t.points.append(p1)
t.points.append(p2)
t.points.append(p3)
print t
con.exec_traj(t)
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment