| Home | Trees | Indices | Help |
|---|
|
|
1 #* Software License Agreement (BSD License)
2 #*
3 #* Copyright (c) 2010, CSIRO Autonomous Systems Laboratory
4 #* All rights reserved.
5 #*
6 #* Redistribution and use in source and binary forms, with or without
7 #* modification, are permitted provided that the following conditions
8 #* are met:
9 #*
10 #* * Redistributions of source code must retain the above copyright
11 #* notice, this list of conditions and the following disclaimer.
12 #* * Redistributions in binary form must reproduce the above
13 #* copyright notice, this list of conditions and the following
14 #* disclaimer in the documentation and/or other materials provided
15 #* with the distribution.
16 #* * Neither the name of the CSIRO nor the names of its
17 #* contributors may be used to endorse or promote products derived
18 #* from this software without specific prior written permission.
19 #*
20 #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 #* POSSIBILITY OF SUCH DAMAGE.
32 #***********************************************************
33
34 # Author: Fred Pauling
35 #$Id$
36
37 from exceptions import InvalidParamterException
38 import string
39 import roslib
40 roslib.load_manifest('sick_ldmrs')
41 import rospy
42 from sick_ldmrs.cfg import ldmrsConfig #config auto-generated by CMake
43 from math import pi
44
46 """ parameter object converts between ROS and ldmrs parameter values and
47 checks validity
48 """
49 deg2ticks = 32 # ticks per degree
50 hz2tickfreq = 256 # tick freq has units 1/256 Hz
51
52 # Device parameter limits
53 #-------------------------------------
54 # Scan frequencies
55 # device uses 1/256 Hz values
56 valid_tick_freqs = [3200, 6400, 12800] # ticks per sec, corresponds to 12.5, 25, and 50 Hz
57 valid_freqs = [12.5, 25, 50] #Hz
58
59 # Start angle limits
60 start_ticks_min = -1919
61 start_angle_min = -59.96 # deg
62 start_ticks_max = 1600
63 start_angle_max = 50.0 # deg
64
65 # End angle limits
66 end_ticks_min = -1920
67 end_angle_min = -60.0 # deg
68 end_ticks_max = 1599
69 end_angle_max = 49.96 # deg
70
71 # Sync angle limits
72 sync_ticks_min = -5760
73 sync_angle_min = -180.0 # deg
74 sync_ticks_max = 5759
75 sync_angle_max = 179.96 # deg
76
77 # set up parameter names. We assume these are in the node private namespace
78 start_angle = 'start_angle'
79 end_angle = 'end_angle'
80 scan_frequency = 'scan_frequency'
81 sync_angle_offset = 'sync_angle_offset'
82 constant_angular_res = 'constant_angular_res'
83 use_first_echo = 'use_first_echo'
84 frame_id_prefix = 'frame_id_prefix'
85
86 # mapping from LD-MRS parameter names to parameter index byte strings
87 parameter_index = {"ip_address":['\x00\x10', "uint32"],
88 "tcp_port":['\x01\x10', "uint32"], # use hex notation
89 "subnet_mask":['\x02\x10', "uint32"], # use hex notation
90 "standard_gateway":['\x03\x10', "uint32"],
91 "data_output_flag":['\x12\x10', "uint16"],
92 "start_angle":['\x00\x11', "int16"],
93 "end_angle":['\x01\x11', "int16"],
94 "scan_frequency":['\x02\x11', "uint16"],
95 "sync_angle_offset":['\x03\x11', "int16"], # actually int14
96 "constant_angular_res":['\x04\x11', "uint16"],
97 "angle_ticks_per_rotation":['\x05\x11', "uint16"]}
98
99 ros_params = {}
100 ldmrs_params = {}
101
102
104 """ Update parameters from passed in config object
105 @param config: dictionary of parameter value pairs
106 @type config: dict {ros_param_string:value }
107 @param level: runlevel (currently unused)
108 @type level: int
109 @param fix_errors: fix invalid input values to be within limits
110 @type fix_errors: bool
111 @return: (possibly updated) config
112 @rtype: dict
113 """
114 # note save values we see an apply_changes True parameter
115 config = self._check_values(config, fix_errors)
116 self.ros_params = config.copy()
117 self._ros_to_ldmrs()
118 return config
119
120
122 ''' Convert ROS parameter values to LDMRS appropriate values
123 '''
124 # copy over the keys and values for everything and convert to ldmrs
125 # values where necessary
126 for k, v in self.ros_params.iteritems():
127 self.ldmrs_params[k] = v
128
129 param_name = self.start_angle
130 self.ldmrs_params[param_name] = int(round(self.ros_params[param_name]*self.deg2ticks))
131
132 param_name = self.end_angle
133 self.ldmrs_params[param_name] = int(round(self.ros_params[param_name]*self.deg2ticks))
134
135 param_name = self.sync_angle_offset
136 self.ldmrs_params[param_name] = int(round(self.ros_params[param_name]*self.deg2ticks))
137
138 param_name = self.constant_angular_res
139 self.ldmrs_params[param_name] = int(self.ros_params[param_name])
140
141 param_name = self.scan_frequency
142 self.ldmrs_params[param_name] = self.valid_tick_freqs[self.ros_params[param_name]]
143
144
146 ''' Check that the parameter values are valid,
147 throw an exception if an invalid combination is detected.
148 Clamping is applied to out of range values and
149 invalid configurations are corrected
150 Collate all parameter setting errors to facilitate faster debugging
151 @param config: configuration dictionary to check for consistency
152 @type config: dict {ros_param_string:value}
153 @param fix_errors: True: adjust the parameters to stay within appropriate bounds,
154 False: raise an exception on error
155 @type fix_errors: bool
156 @return: config (possibly updated)
157 @rtype: dict
158 '''
159
160 # flag exceptions by adding messages to the list
161 # no exception is raised if the list is empty
162 exception_msg = [] # list of exception messages
163
164 for param_name, value in config.iteritems():
165
166 # handle value < minimum allowed
167 min = None if not param_name in ldmrsConfig.min else ldmrsConfig.min[param_name]
168 if min is not None and min != '' and value < min:
169 msg = str(err_num) + ": Parameter '%s' value %s exceeds minimum allowed value of %s. "\
170 %(param_name, str(value), str(min))
171 if fix_errors:
172 msg += "Using the minimum allowed value of %s"%str(min)
173 rospy.logwarn(msg)
174 config[param_name] = min
175 else:
176 rospy.logerr(msg)
177 raise InvalidParameterException(msg)
178 exception_msg.append(msg)
179
180 # handle value > maximum allowed
181 max = None if not param_name in ldmrsConfig.max else ldmrsConfig.max[param_name]
182 if max is not None and max != '' and value > max:
183 msg = str(err_num) + ": Parameter '%s' value %s exceeds maximum allowed value of %s. "\
184 %(param_name, str(value), str(max))
185 if fix_errors:
186 msg += "Using the maximum allowed value of %s"%str(max)
187 rospy.logwarn(msg)
188 config[param_name] = max
189 else:
190 rospy.logerr(msg)
191 raise InvalidParameterException(msg)
192
193 # check that the angular_res_type is appropriate, given the scan frequency
194 scan_freq = config[self.scan_frequency]
195 res_type = config[self.constant_angular_res]
196 if not res_type and (scan_freq != 0):
197 rospy.logerr("Focused angular resolution mode is only available when scan frequency is 12.5Hz but " \
198 "current frequency is " + str(self.valid_freqs[scan_freq]))
199 if fix_errors:
200 rospy.logwarn("Changing angular resolution type to constant and keeping scan_frequency at %d"%self.valid_freqs[scan_freq])
201 config[self.constant_angular_res] = True
202 else:
203 rospy.logerr("Either change the scan frequency to 12.5Hz (value 0) or set the constant_angular_res parameter")
204 raise InvalidParameterException('Illegal scan_freqency / constant_angular_res parameter combination')
205
206 # check that start angle > end_angle
207 if config["start_angle"] <= config["end_angle"]:
208 rospy.logerr("Start angle must be greater than end angle.")
209 if fix_errors:
210 rospy.logwarn("Using default values of start_angle=%s, end_angle=%s"
211 %(ldmrsConfig.defaults[self.start_angle], ldmrsConfig.defaults[self.end_angle]))
212 config[self.start_angle] = ldmrsConfig.defaults[self.start_angle]
213 config[self.end_angle] = ldmrsConfig.defaults[self.end_angle]
214 else:
215 raise InvalidParamterException("End angle parameter is greater than start angle.")
216
217 return config
218
220 ''' Get the value of ldmrs parameter <param_name> (in LDMRS format)
221 @param param_name: ldmrs parameter name to get the value of
222 @type param_name: string
223 @return: ldmrs parameter value for parmeter param_name
224 @rtype: type of parameter param_name
225 '''
226 return None if param_name not in self.ldmrs_params else self.ldmrs_params[param_name]
227
229 ''' Get the value of ROS parameter <param_name>
230 @param param_name: ROS parameter name to get the value of
231 @type param_name: string
232 @return: ROS parameter value for parmeter param_name
233 @rtype: type of parameter param_name
234 '''
235 return None if param_name not in self.ros_params else self.ros_params[param_name]
236
| Home | Trees | Indices | Help |
|---|
| Generated by Epydoc 3.0.1 on Fri Mar 1 15:16:54 2013 | http://epydoc.sourceforge.net |