def spin(self):
rospy.sleep(10)
raw_input("Ready to start movement? (press enter)")
(pos, rot) = self._listener.lookupTransform(self._base_frame, self._ee_frame, rospy.Time(0))
current_pose = {'position':pos, 'orientation': rot}
rate = rospy.Rate(self._publish_rate)
while not rospy.is_shutdown():
# run path
count = 1
for p in self._data['path']:
print 'Pose #{}'.format(count)
count += 1
target_pose = p
linear_path = self._generate_linear_path(current_pose,target_pose)
for lp in linear_path:
self._ee_goals_pub.publish(EEPoseGoals(ee_poses=[self._format_pose(lp)]))
rate.sleep()
current_pose = target_pose
rospy.sleep(self._pose_switch_delay)
# if done
if not self._repeat:
break
xopt = relaxedIK.solve(goal_pos, goal_quat, overwrite_joints,
overwrite_joint_values)
js = joint_state_define(xopt)
if js == None:
js = JointState()
js.name = joint_ordering
for x in xopt:
js.position.append(x)
now = rospy.Time.now()
js.header.stamp.secs = now.secs
js.header.stamp.nsecs = now.nsecs
js_pub.publish(js)
ee_pose_goals = EEPoseGoals()
ee_pose_goals.header.seq = idx
for i in range(num_ee):
p = Pose()
curr_goal_pos = goal_pos[i]
curr_goal_quat = goal_quat[i]
p.position.x = curr_goal_pos[0]
p.position.y = curr_goal_pos[1]
p.position.z = curr_goal_pos[2]
p.orientation.w = curr_goal_quat[0]
p.orientation.x = curr_goal_quat[1]
p.orientation.y = curr_goal_quat[2]
p.orientation.z = curr_goal_quat[3]
ee_pose_goals.ee_poses.append(p)
vector3_r = copy.deepcopy(glob_vector3_r)
quaternion_l = copy.deepcopy(glob_quaternion_l)
vector3_l = copy.deepcopy(glob_vector3_l)
clutch_down = glob_clutch_down
length_error = False
if not len(vector3_r) == 3: length_error = True
if not len(quaternion_r) == 4: length_error = True
if length_error:
rate.sleep()
continue
eepg = EEPoseGoals()
pose_r = Pose()
pose_l = Pose()
left = True
if not len(vector3_l) == 3 or not len(quaternion_l) == 4:
left = False
if not left:
pose_l = Pose()
pose_l.position.x = 0
pose_l.position.y = 0
pose_l.position.z = 0
pose_l.orientation.w = 1
pose_l.orientation.x = 0