Commit 2e3651c9 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

read status of joint trajectory from controller in order to do things while executing (#10)

parent 7f34b365
......@@ -21,6 +21,7 @@ from trajectory_msgs.msg import (
JointTrajectoryPoint,
JointTrajectory
)
from actionlib_msgs.msg import GoalStatusArray
# MOVEit
import moveit_commander
......@@ -76,6 +77,8 @@ class Controller(object):
queue_size=1)
self.scaledPosTraj_pub = rospy.Publisher('scaled_pos_traj_controller/command',JointTrajectory,
queue_size=1)
self.motion_feedback_sub = rospy.Subscriber('scaled_pos_traj_controller/follow_joint_trajectory/status',GoalStatusArray,
self._update_motion_status)
time.sleep(0.1)
if not disable_extruder:
......@@ -101,6 +104,7 @@ class Controller(object):
self.retractionDist_mm = retractionDist_mm
self.retractionTime_s = retractionDist_mm/retractionSpeed_mmpmin * 60.0
self.extruderReady = False
self.traj_complete = False
# Start extruder
if not disable_extruder:
......@@ -160,7 +164,11 @@ class Controller(object):
plan = RobotTrajectory()
plan.joint_trajectory = scaledTraj
self.group.execute(plan)
self.group.execute(plan, wait=False)
s = time.time()
while time.time() - s < traj.points[-1].time_from_start.to_sec(): #not self.traj_complete:
pass
return True
......@@ -278,6 +286,11 @@ class Controller(object):
def _extruderReady_cb(self,ready_boolMsg):
self.extruderReady = ready_boolMsg.data
def _update_motion_status(self, msg):
if len(msg.status_list)>0:
self.traj_complete = (msg.status_list[-1].status==3 or
msg.status_list[-1].status==2)
def getR_ET():
return Rotation.from_euler('XYZ',[-45,-90,0],
......
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