Commit 7d4e8c57 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

separate function for extrusion travel (#10)

parent dcc5df41
......@@ -30,6 +30,7 @@ from moveit_msgs.msg._RobotTrajectory import RobotTrajectory
## Our libraries
from extrudex.msg._ExtruderControl import ExtruderControl
from extrudex.srv._Hotend import Hotend
from utils import load_plan, JTraj2ROS
class Controller(object):
"""
......@@ -132,6 +133,41 @@ class Controller(object):
return t
def exec_ctraj(self,jtraj, wait=True):
"""
Using the
jtraj: JTrajectory object to execute
Returns True if the JointTrajectory is executed; False otherwise.
"""
plan = RobotTrajectory()
plan.joint_trajectory = JTraj2ROS(jtraj)
# plan.joint_trajectory.points = plan.joint_trajectory.points[1:]
rospy.loginfo('Sending trajectory to UR5e')
self.traj_complete = False
self.group.execute(plan, wait=False)
#if jtraj.contour is not None:
# print("---")
# print("THIS NEXT TRAJ REQUIRES EXTRUSION")
# print(jtraj.contour.Ext)
# print(jtraj.contour.time)
# print("---")
#self.extrude()
if wait:
s = time.time()
t = jtraj.time[-1]
while not self.traj_complete:
if time.time() - s > t + 3:
Exception("traj_complete failed")
break
self.traj_complete = False
return True
def exec_traj(self,traj):
"""
Executes a JointTrajectory object
......@@ -164,15 +200,7 @@ class Controller(object):
plan = RobotTrajectory()
plan.joint_trajectory = scaledTraj
self.group.execute(plan, wait=False)
s = time.time()
t = traj.points[-1].time_from_start.to_sec()
while not self.traj_complete:
if time.time() - s > t + 3:
Exception("traj_complete failed")
break
self.traj_complete = False
self.group.execute(plan, wait=True)
return True
......
......@@ -592,7 +592,8 @@ class KeyboardController(Controller):
if __name__=="__main__":
con = KeyboardController(disableExtruder=True)
#con = KeyboardController(disableExtruder=True)
con = KeyboardController()
print('Collecting configrations...')
configuations = con.prompt_configurations()
......
......@@ -34,13 +34,12 @@ def execute_plan(plan, controller):
controller.group.go(plan.trajs[0][0].positions[0])
# execute trajectories in plan
for t in plan.trajs[0]:
jt = JTraj2ROS(t)
try:
controller.exec_traj(jt)
except:
print("Failed trajectory Execution")
break
for i, t in enumerate(plan.trajs[0]):
i+=1
if i==1:
print("skipping first traj")
continue
controller.exec_ctraj(t)
return
......
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