config_manager.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2008, Willow Garage, Inc.
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of Willow Garage, Inc. nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 
34 import roslib; roslib.load_manifest('calibration_launch')
35 import rospy
36 import yaml
37 import sys
38 import actionlib
39 import joint_states_settler.msg
40 import monocam_settler.msg
41 import image_cb_detector.msg
42 import laser_cb_detector.msg
43 import interval_intersection.msg
44 import time
45 from trajectory_msgs.msg import JointTrajectory
46 from trajectory_msgs.msg import JointTrajectoryPoint
47 
48 # Puts the system in a specific configuration, in preparation for collecting a sample
50  def __init__(self, cam_config, chain_config, laser_config, controller_config):
51 
52  # Set up Cameras
53  rospy.logdebug("Constructing Cameras:")
54  self._cam_managers = dict()
55  for name in cam_config.keys():
56  rospy.logdebug(" Constructing CamID [%s]" % name)
57  self._cam_managers[name] = CameraConfigManager(name, cam_config[name])
58 
59  # Set up Chains
60  rospy.logdebug("Constructing Chains:")
61  self._chain_managers = dict()
62  for name in chain_config.keys():
63  rospy.logdebug(" Constructing ChainID [%s]" % name)
64  self._chain_managers[name] = ChainConfigManager(name, chain_config[name])
65 
66  # Set up lasers
67  rospy.logdebug("Constructing Lasers:")
68  self._laser_managers = dict()
69  for laser_id in laser_config.keys():
70  rospy.logdebug(" Constructing LaserID [%s]" % laser_id)
71  self._laser_managers[laser_id] = LaserConfigManager(laser_id, laser_config[laser_id])
72 
73  # Set up Controllers
74  rospy.logdebug("Constructing Controllers:")
75  self._controller_managers = dict()
76  for controller_id in controller_config.keys():
77  rospy.logdebug(" Constructing ControllerID [%s]" % controller_id)
78  self._controller_managers[controller_id] = ControllerCmdManager(controller_id, controller_config[controller_id])
79 
80  # Set up interval_intersection
81  self._intersect_ac = actionlib.SimpleActionClient("interval_intersection_config", interval_intersection.msg.ConfigAction)
82 
83  def reconfigure(self, config):
84  # Reconfigure Interval Intersection
85 
86  intersect_goal = interval_intersection.msg.ConfigGoal();
87  try: # handle case where there are no lasers
88  intersect_goal.topics = [x['cam_id'] for x in config["camera_measurements"]] + [x['chain_id'] for x in config["joint_measurements"]] + [x['laser_id'] for x in config["laser_measurements"]]
89  except KeyError:
90  intersect_goal.topics = [x['cam_id'] for x in config["camera_measurements"]] + [x['chain_id'] for x in config["joint_measurements"]]
91  self._intersect_ac.send_goal(intersect_goal)
92 
93  # Reconfigure the cameras
94  rospy.logdebug("Reconfiguring The Cameras")
95  for cur_cam in config["camera_measurements"]:
96  self._cam_managers[cur_cam["cam_id"]].reconfigure(cur_cam["config"])
97 
98  # Reconfigure the chains
99  rospy.logdebug("Reconfiguring The Chains")
100  for cur_chain in config["joint_measurements"]:
101  self._chain_managers[cur_chain["chain_id"]].reconfigure(cur_chain["config"])
102 
103  # Reconfigure the lasers
104  rospy.logdebug("Reconfiguring The Lasers")
105  try:
106  for cur_laser in config["laser_measurements"]:
107  self._laser_managers[cur_laser["laser_id"]].reconfigure(cur_laser["config"])
108  except KeyError:
109  pass # if no lasers, nothing to do.
110 
111  # Send controller commands
112  rospy.logdebug("Sending Controller Commands")
113  for cur_controller in config["joint_commands"]:
114  self._controller_managers[cur_controller["controller"]].send_command(cur_controller)
115 
116  def stop(self):
117  pass
118 
119 # Handles changing the configuration of a joint_states_settler
120 class ChainConfigManager:
121  def __init__(self, chain_id, configs):
122  self._chain_id = chain_id
123  self._configs = configs
124  if not self.validate():
125  raise Exception("Invalid chain description")
126 
127  # Initialize the ActionClient and state
128  self._settler_ac = actionlib.SimpleActionClient(self._configs["settler_config"], joint_states_settler.msg.ConfigAction)
129  self._state = "idle"
130 
131  # Check to make sure that config dict has all the necessary fields. TODO: Currently a stub
132  def validate(self):
133  return True
134 
135  # Reconfigure this chain's processing pipeline to the specified configuration. Do nothing if
136  # we're already in the correct configuration
137  def reconfigure(self, next_config_name):
138  if self._state == next_config_name:
139  rospy.logdebug(" %s: Configured correctly as [%s]" % (self_.chain_id, self._state))
140  else:
141  rospy.logdebug(" %s: Need to transition [%s] -> [%s]" % (self._chain_id, self._state, next_config_name))
142 
143  next_config = self._configs["configs"][next_config_name]
144  settler_config = next_config["settler"]
145  # Convert the settler's configuration from a dictionary to a ROS message
146  goal = joint_states_settler.msg.ConfigGoal()
147  goal.joint_names = settler_config["joint_names"]
148  goal.tolerances = settler_config["tolerances"]
149  goal.max_step = rospy.Duration(settler_config["max_step"])
150  goal.cache_size = settler_config["cache_size"]
151 
152  # Send the goal out
153  self._settler_ac.send_goal(goal)
154 
155  # TODO: Need to add code that waits for goal to activate
156 
157 
158 # Handles changing the configuration of the image pipeline associated with a camera
160  def __init__(self, cam_id, configs):
161  self._cam_id = cam_id
162  self._configs = configs
163  if not self.validate():
164  raise Exception("Invalid camera description")
165 
166  # Initialize the ActionClients and state
167  self._settler_ac = actionlib.SimpleActionClient(self._configs["settler_config"], monocam_settler.msg.ConfigAction)
168  self._led_detector_ac = actionlib.SimpleActionClient(self._configs["cb_detector_config"], image_cb_detector.msg.ConfigAction)
169  self._state = "idle"
170 
171  # Check to make sure that config dict has all the necessary fields. TODO: Currently a stub
172  def validate(self):
173  return True
174 
175  # Reconfigure this chain's processing pipeline to the specified configuration. Do nothing if
176  # we're already in the correct configuration
177  def reconfigure(self, next_config_name):
178  if self._state == next_config_name:
179  rospy.logdebug(" %s: Configured correctly as [%s]" % (self_.cam_id, self._state))
180  else:
181  rospy.logdebug(" %s: Need to transition [%s] -> [%s]" % (self._cam_id, self._state, next_config_name))
182 
183  next_config = self._configs["configs"][next_config_name]
184 
185  # Send the Settler's Goal
186  settler_config = next_config["settler"]
187  goal = monocam_settler.msg.ConfigGoal()
188  goal.tolerance = settler_config["tolerance"]
189  goal.ignore_failures = settler_config["ignore_failures"]
190  goal.max_step = rospy.Duration(settler_config["max_step"])
191  goal.cache_size = settler_config["cache_size"]
192  self._settler_ac.send_goal(goal)
193 
194  # Send the CB Detector Goal
195  cb_detector_config = next_config["cb_detector"]
196  if not cb_detector_config["active"]:
197  rospy.logerr("Can't deal with inactive cb_detector")
198  goal = image_cb_detector.msg.ConfigGoal()
199  goal.num_x = cb_detector_config["num_x"]
200  goal.num_y = cb_detector_config["num_y"]
201  goal.spacing_x = 1.0 # Hardcoded garbage value. Should eventually be removed from the msg
202  goal.spacing_y = 1.0 # Hardcoded garbage value. Should eventually be removed from the msg
203  goal.width_scaling = cb_detector_config["width_scaling"]
204  goal.height_scaling = cb_detector_config["height_scaling"]
205  goal.subpixel_window = cb_detector_config["subpixel_window"]
206  goal.subpixel_zero_zone = cb_detector_config["subpixel_zero_zone"]
207  self._led_detector_ac.send_goal(goal)
208 
209  # Send the LED Detector Goal
210  led_detector_config = next_config["led_detector"]
211  if led_detector_config["active"]:
212  rospy.logerr("Can't deal with an active led_detector")
213 
214  # TODO: Need to add code that waits for goal to activate
215 
216 # Handles changing the configuration of the image pipeline associated with a camera
218  def __init__(self, laser_id, configs):
219  self._laser_id = laser_id
220  self._configs = configs
221  if not self.validate():
222  raise Exception("Invalid laser description")
223 
224  # Initialize the ActionClients and state
225  self._settler_ac = actionlib.SimpleActionClient(self._configs["settler_config"], monocam_settler.msg.ConfigAction)
226  self._cb_detector_ac = actionlib.SimpleActionClient(self._configs["cb_detector_config"], laser_cb_detector.msg.ConfigAction)
227  self._state = "idle"
228 
229  # Check to make sure that config dict has all the necessary fields. TODO: Currently a stub
230  def validate(self):
231  return True
232 
233  # Reconfigure this chain's processing pipeline to the specified configuration. Do nothing if
234  # we're already in the correct configuration
235  def reconfigure(self, next_config_name):
236  if self._state == next_config_name:
237  rospy.logdebug(" %s: Configured correctly as [%s]" % (self_.laser_id, self._state))
238  else:
239  rospy.logdebug(" %s: Need to transition [%s] -> [%s]" % (self._laser_id, self._state, next_config_name))
240 
241  next_config = self._configs["configs"][next_config_name]
242 
243  # Send the Settler's Goal
244  settler_config = next_config["settler"]
245  goal = monocam_settler.msg.ConfigGoal()
246  goal.tolerance = settler_config["tolerance"]
247  goal.ignore_failures = settler_config["ignore_failures"]
248  goal.max_step = rospy.Duration(settler_config["max_step"])
249  goal.cache_size = settler_config["cache_size"]
250  self._settler_ac.send_goal(goal)
251 
252  # Send the CB Detector Goal
253  cb_detector_config = next_config["cb_detector"]
254  if not cb_detector_config["active"]:
255  rospy.logerr("Not sure yet how to deal with inactive cb_detector")
256  goal = laser_cb_detector.msg.ConfigGoal()
257  goal.num_x = cb_detector_config["num_x"]
258  goal.num_y = cb_detector_config["num_y"]
259  goal.spacing_x = cb_detector_config["spacing_x"]
260  goal.spacing_y = cb_detector_config["spacing_y"]
261  goal.width_scaling = cb_detector_config["width_scaling"]
262  goal.height_scaling = cb_detector_config["height_scaling"]
263  goal.subpixel_window = cb_detector_config["subpixel_window"]
264  goal.subpixel_zero_zone = cb_detector_config["subpixel_zero_zone"]
265  goal.flip_horizontal = cb_detector_config["flip_horizontal"]
266  goal.min_intensity = cb_detector_config["min_intensity"]
267  goal.max_intensity = cb_detector_config["max_intensity"]
268  self._cb_detector_ac.send_goal(goal)
269 
270  # TODO: Need to add code that waits for goal to activate
271 
272 
273 
274 # Handles publishing commands to a trajectory controller
276  def __init__(self, controller_id, config):
277  self._controller_id = controller_id
278  self._config = config
279  if not self.validate():
280  raise Exception("Invalid chain description")
281 
282  # Initialize the Publisher
283  self._pub = rospy.Publisher(self._config["topic"], JointTrajectory)
284 
285  # Check to make sure that config dict has all the necessary fields. TODO: Currently a stub
286  def validate(self):
287  return True
288 
289  # Reconfigure this chain's processing pipeline to the specified configuration. Do nothing if
290  # we're already in the correct configuration
291  def send_command(self, cmd):
292  rospy.logdebug(" Sending cmd to controller [%s]"%self._controller_id)
293  cmd_msg = JointTrajectory()
294  cmd_msg.header.stamp = rospy.Time().now()
295  cmd_msg.joint_names = self._config["joint_names"]
296  cmd_msg.points = [self._build_segment(x) for x in cmd["segments"]]
297  self._pub.publish(cmd_msg)
298 
299  def _build_segment(self, config):
300  segment = JointTrajectoryPoint()
301  segment.positions = config["positions"]
302  segment.velocities = [0] * len(config["positions"])
303  segment.time_from_start = rospy.Duration(config["duration"])
304  return segment
305 
306 
307 
def __init__(self, cam_config, chain_config, laser_config, controller_config)


calibration_launch
Author(s): Michael Ferguson
autogenerated on Thu Jun 6 2019 19:17:33