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()