3 PKG =
"pr2_mechanism_controllers" 5 import roslib; roslib.load_manifest(PKG)
12 from std_msgs
import *
14 from pr2_msgs.msg
import LaserTrajCmd
16 from time
import sleep
20 send_periodic_cmd.py [controller] 24 if __name__ ==
'__main__':
29 controller = sys.argv[1]
30 cmd.header = rospy.Header(
None,
None,
None)
31 cmd.profile =
"blended_linear" 37 cmd.position = [-.7, 1.2, -.7]
38 cmd.time_from_start = [0.0, dur, dur+1]
39 cmd.time_from_start = [rospy.Duration.from_sec(x)
for x
in cmd.time_from_start]
41 cmd.max_acceleration = 30
43 print(
'Sending Command to %s: ' % controller)
44 print(
' Profile Type: %s' % cmd.profile)
45 print(
' Pos: %s ' %
','.join([
'%.3f' % x
for x
in cmd.position]))
46 print(
' Time: %s' %
','.join([
'%.3f' % x.to_sec()
for x
in cmd.time_from_start]))
47 print(
' MaxRate: %f' % cmd.max_velocity)
48 print(
' MaxAccel: %f' % cmd.max_acceleration)
50 rospy.wait_for_service(controller +
'/set_traj_cmd')
51 s = rospy.ServiceProxy(controller +
'/set_traj_cmd', SetLaserTrajCmd)
52 resp = s.call(SetLaserTrajCmdRequest(cmd))
54 print(
'Command sent!')
55 print(
' Resposne: %f' % resp.start_time.to_sec())
def print_usage(exit_code=0)