def goto_pose(self, x, y, theta, frame_id):
trans,rot = conv.xya_to_trans_rot([x,y,theta])
pose = conv.trans_rot_to_pose(trans, rot)
ps = gm.PoseStamped()
ps.pose = pose
ps.header.frame_id = frame_id
ps.header.stamp = rospy.Time(0)
goal = mbm.MoveBaseGoal()
goal.target_pose = ps
rospy.loginfo('Sending move base goal')
finished = self.action_client.send_goal_and_wait(goal)
rospy.loginfo('Move base action returned %d.' % finished)
return finished
Globals.pr2.join_all()
return ExecTrajectoryResponse(success=True)
if __name__ == "__main__":
if args.test:
if rospy.get_name() == "/unnamed":
rospy.init_node("test_exec_verb_traj_service", disable_signals=True)
Globals.setup()
import h5py
F = h5py.File("/home/joschu/python/lfd/data/verbs.h5", "r")
demo = F["push"]["demos"]["0"]
req = ExecTrajectoryRequest()
traj = req.traj
traj.gripper0_poses.poses = [
conversions.trans_rot_to_pose(trans, rot)
for (trans, rot) in zip(demo["l_gripper_xyzs"], demo["l_gripper_quats"])
]
traj.gripper0_angles = demo["l_gripper_angles"]
traj.narms = 1
callback(req)
else:
rospy.init_node("exec_verb_traj_service", disable_signals=True)
Globals.setup()
service = rospy.Service("exec_verb_traj", ExecTrajectory, callback)
rospy.spin()
