start_laser.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 ### copy from pr2_move_base/scripts/pr2_move_base.py
00003 import roslib; roslib.load_manifest('pr2_move_base')
00004 import rospy
00005 
00006 from pr2_msgs.msg import LaserTrajCmd
00007 from pr2_msgs.srv import SetLaserTrajCmd
00008 import dynamic_reconfigure.client
00009 
00010 def set_tilt_profile(position, time_from_start):
00011     cmd = LaserTrajCmd()
00012     cmd.profile = 'blended_linear'
00013     cmd.position = position
00014     cmd.time_from_start = [rospy.Time.from_sec(x) for x in time_from_start]
00015     cmd.max_velocity = 10
00016     cmd.max_acceleration = 30
00017     try:
00018         tilt_profile_client.call(cmd)
00019     except rospy.ServiceException, e:
00020         rospy.logerr("Couldn't set the profile on the laser. Exception %s" % e)
00021         return False
00022     return True
00023 
00024 def configure_laser():
00025     #TODO: remove hack to get things working in gazebo
00026     try:
00027         rospy.wait_for_service('tilt_hokuyo_node/set_parameters', 10.0)
00028     except rospy.exceptions.ROSException, e:
00029         rospy.logerr("Couldn't set parameters %s" % e)
00030         return
00031     #end TODO
00032 
00033     client = dynamic_reconfigure.client.Client('tilt_hokuyo_node')
00034     global old_config
00035     old_config = client.get_configuration(2.0)
00036     new_config = {'skip': 0, 'intensity': 0, 'min_ang': -1.57, 'max_ang': 1.57, 'calibrate_time': 1, 'cluster': 1, 'time_offset': 0.0}
00037     rospy.loginfo('Setting laser to the navigation configuration: %s' % new_config)
00038     client.update_configuration(new_config)
00039 
00040 if __name__ == '__main__':
00041     name = 'setting_laser'
00042     rospy.init_node(name)
00043 
00044     #create a client to the tilt laser
00045     rospy.wait_for_service('laser_tilt_controller/set_traj_cmd')
00046     tilt_profile_client = rospy.ServiceProxy('laser_tilt_controller/set_traj_cmd', SetLaserTrajCmd)
00047 
00048     ## set_tilt_profile([1.05,  -.7, 1.05], [0.0, 1.8, 2.0125 + .3]) ## original
00049     set_tilt_profile([1.05,  -.7, 1.05], [0.0, 2.4, 2.4 + .2125 + .3])
00050     configure_laser()


pr2eus_tutorials
Author(s): Kei Okada
autogenerated on Thu Jun 6 2019 21:52:27