def set_tilt_profile(position, time_from_start): cmd = LaserTrajCmd() cmd.profile = 'blended_linear' cmd.position = position cmd.time_from_start = [rospy.Time.from_sec(x) for x in time_from_start] cmd.max_velocity = 10 cmd.max_acceleration = 30 try: tilt_profile_client.call(cmd) except rospy.ServiceException, e: rospy.logerr("Couldn't set the profile on the laser. Exception %s" % e) return False
def print_usage(exit_code = 0): print '''Usage: laser_traj_cmd.py [controller] ''' sys.exit(exit_code) if __name__ == '__main__': if len(sys.argv) < 2: print_usage() rospy.init_node('laser_tilt_commander', anonymous=True) cmd = LaserTrajCmd() controller = sys.argv[1] cmd.header.stamp = rospy.get_rostime() cmd.profile = "linear" #cmd.pos = [1.0, .26, -.26, -.7, -.7, -.26, .26, 1.0, 1.0] d = .025 #cmd.time = [0.0, 0.4, 1.0, 1.1, 1.1+d, 1.2+d, 1.8+d, 2.2+d, 2.2+2*d] cmd.position = [1.5, -.7, 1.5] #cmd.time_from_start = [0.0, 1.0, 2.0] #[0.0, 0.5, 1.0] cmd.time_from_start = [0.0, 4.0, 8.0] #[0.0, 0.5, 1.0] cmd.time_from_start = [rospy.Time.from_sec(x) for x in cmd.time_from_start] cmd.max_velocity = 5 cmd.max_acceleration = 5 print 'Sending Command to %s: ' % controller print ' Profile Type: %s' % cmd.profile print ' Pos: %s ' % ','.join(['%.3f' % x for x in cmd.position]) print ' Time: %s' % ','.join(['%.3f' % x.to_sec() for x in cmd.time_from_start])
def print_usage(exit_code=0): print '''Usage: send_periodic_cmd.py [controller] ''' sys.exit(exit_code) if __name__ == '__main__': if len(sys.argv) < 2: print_usage() cmd = LaserTrajCmd() controller = sys.argv[1] cmd.header = rospy.Header(None, None, None) cmd.profile = "blended_linear" cycle_time = rospy.get_param('~cycle_time', 5.0) min_tilt_angle = rospy.get_param('~min_tilt_angle', -0.7) max_tilt_angle = rospy.get_param('~max_tilt_angle', 1.0) cmd.position = [min_tilt_angle, max_tilt_angle, min_tilt_angle] cmd.time_from_start = [0.0, cycle_time - 1.0, cycle_time] cmd.time_from_start = [ rospy.Duration.from_sec(x) for x in cmd.time_from_start ] cmd.max_velocity = 10 cmd.max_acceleration = 30 print 'Sending Command to %s: ' % controller print ' Profile Type: %s' % cmd.profile
import sys import os import string import rospy from std_msgs import * from pr2_msgs.msg import LaserTrajCmd from pr2_msgs.srv import * from time import sleep cmd = LaserTrajCmd() controller = 'laser_tilt_controller' cmd.header = rospy.Header(None, None, None) cmd.profile = "blended_linear" #cmd.pos = [1.0, .26, -.26, -.7, -.7, -.26, .26, 1.0, 1.0] d = .025 #cmd.time = [0.0, 0.4, 1.0, 1.1, 1.1+d, 1.2+d, 1.8+d, 2.2+d, 2.2+2*d] dur = 15; cmd.position = [-1.0, 0, 1] cmd.time_from_start = [0.0, dur, dur+1] cmd.time_from_start = [rospy.Duration.from_sec(x) for x in cmd.time_from_start] cmd.max_velocity = 10 cmd.max_acceleration = 30 print 'Sending Command to %s: ' % controller print ' Profile Type: %s' % cmd.profile print ' Pos: %s ' % ','.join(['%.3f' % x for x in cmd.position]) print ' Time: %s' % ','.join(['%.3f' % x.to_sec() for x in cmd.time_from_start])