jsk_tilt_laser: tilt_laser_assembler.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import rospy
00003 try:
00004     from laser_assembler.srv import *
00005 except:
00006     import roslib; roslib.load_manifest("laser_assembler");
00007     from laser_assembler.srv import *
00008 
00009 class AssembleCaller:
00010     cloud_pub = None
00011     joint_sub = None
00012     assemble_srv = None
00013     command_pub = None
00014     prev_angle = None
00015     max_angle = None
00016     min_angle = None
00017     lower_threshold = None
00018     upper_threshold = None
00019     scan_time = None ###
00020 
00021     def init(self):
00022         rospy.init_node('tilt_laser_assembler')
00023         self.joint_name = rospy.get_param("~tilt_joint_name", "tilt_joint")
00024         self.cloud_pub = rospy.Publisher('assemble_cloud', sensor_msgs.msg.PointCloud2)
00025         self.command_pub = rospy.Publisher('/tilt_controller/command', std_msgs.msg.Float64)
00026         self.joint_sub = rospy.Subscriber('joint_states', sensor_msgs.msg.JointState, self.joint_callback)
00027         self.assemble_srv = rospy.ServiceProxy('assemble_scans2', AssembleScans2)
00028 
00029         if not self.max_angle:
00030             self.max_angle = rospy.get_param('~tilt_joint_max_angle', default=0.70)
00031 
00032         if not self.min_angle:
00033             self.min_angle = rospy.get_param('~tilt_joint_min_angle',default=-0.95)
00034         if not self.scan_time:
00035             self.scan_time = 8.0
00036         if not self.lower_threshold:
00037             self.lower_threshold = self.min_angle
00038         if not self.upper_threshold:
00039             self.upper_threshold = self.max_angle
00040 
00041     def move_to_angle(self, angle):
00042         self.command_pub.publish(std_msgs.msg.Float64(angle))
00043 
00044     def joint_callback(self, msg):
00045         pos = None
00046         try:
00047             pos = msg.position[msg.name.index(self.joint_name)] ### 
00048             rospy.logdebug('pos = %f'%pos)
00049         except:
00050             # do nothing
00051             rospy.logdebug('exept')
00052 
00053         if pos:
00054             if not self.prev_angle:
00055                 self.prev_angle = pos
00056                 if pos - self.min_angle > self.max_angle - pos:
00057                     self.prev_time = msg.header.stamp
00058                     self.move_to_angle(self.max_angle + 0.01)
00059                 else:
00060                     self.prev_time = msg.header.stamp
00061                     self.move_to_angle(self.min_angle - 0.01)
00062                 return
00063             if pos > self.max_angle:
00064                 self.move_to_angle(self.min_angle - 0.01)
00065             if self.prev_angle < self.upper_threshold and pos > self.upper_threshold:
00066                 self.scan_and_publish(self.prev_time, msg.header.stamp)
00067 
00068             if self.prev_angle > self.lower_threshold and pos < self.lower_threshold:
00069                 self.scan_and_publish(self.prev_time, msg.header.stamp)
00070 
00071             if pos < self.min_angle:
00072                 self.move_to_angle(self.max_angle + 0.01)
00073             # update timestamp
00074             if pos < self.min_angle or pos > self.max_angle:
00075                 self.prev_time = msg.header.stamp
00076             self.prev_angle = pos
00077 
00078     def scan_and_publish(self, begin, end):
00079         req = AssembleScans2Request()
00080         req.begin = begin
00081         req.end = end
00082         try:
00083             ret = self.assemble_srv(req.begin, req.end)
00084             if ret:
00085                 rospy.logdebug('publish')
00086                 self.cloud_pub.publish(ret.cloud)
00087         except:
00088             # do nothing
00089             rospy.logerr('error calling service')
00090 
00091     def spin(self):
00092         rospy.spin()
00093 
00094 if __name__=='__main__':
00095     a = AssembleCaller()
00096     a.init()
00097     a.spin()