def filter( self, trajectory ):
# rospy.loginfo( 'filtering trajectory:\n %s'%trajectory )
request = FilterJointTrajectoryWithConstraintsRequest()
request.allowed_time = rospy.Duration.from_sec(5.0);
request.trajectory = trajectory
try:
response = self.proxy( request )
if response.error_code.val == response.error_code.SUCCESS:
return response.trajectory
else:
rospy.logerr( 'Trajectory was not filtered! (error: %s)'%response.error_code.val )
return None
except rospy.ServiceException, e:
rospy.logerr( 'service call failed: %s'%e)
return None
def execute_trajectory(self, trajectory, arm, wait=False):
command = JointTrajectory()
command.joint_names = ['%s_shoulder_pan_joint' % arm[0],
'%s_shoulder_lift_joint' % arm[0],
'%s_upper_arm_roll_joint' % arm[0],
'%s_elbow_flex_joint' % arm[0],
'%s_forearm_roll_joint' % arm[0],
'%s_wrist_flex_joint' % arm[0],
'%s_wrist_roll_joint' % arm[0]]
if arm[0] == "l":
client = self.l_arm_client
elif arm[0] == "r":
client = self.r_arm_client
for jvals in trajectory:
command.points.append(JointTrajectoryPoint(
positions=jvals,
velocities = [],
accelerations = [],
time_from_start = rospy.Duration(0)))
#command.header.stamp = rospy.Time.now()
rospy.loginfo("Sending request to trajectory filter")
req = FilterJointTrajectoryWithConstraintsRequest()
req.trajectory = command
req.allowed_time = rospy.Duration(1.)
reply = self.filter_service.call(req)
if reply.error_code.val != reply.error_code.SUCCESS:
rospy.logerr("Filter trajectory returns %d"%reply.error_code.val)
return
goal = JointTrajectoryGoal()
goal.trajectory = reply.trajectory
if arm[0] == "l":
client.send_goal(goal, done_cb=self.__l_arm_done_cb)
elif arm[0] == "r":
client.send_goal(goal, done_cb=self.__r_arm_done_cb)
if wait:
client.wait_for_result()
