config_manager.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 import roslib; roslib.load_manifest('calibration_launch')
00035 import rospy
00036 import yaml
00037 import sys
00038 import actionlib
00039 import joint_states_settler.msg
00040 import monocam_settler.msg
00041 import image_cb_detector.msg
00042 import laser_cb_detector.msg
00043 import interval_intersection.msg
00044 import time
00045 from trajectory_msgs.msg import JointTrajectory
00046 from trajectory_msgs.msg import JointTrajectoryPoint
00047 
00048 # Puts the system in a specific configuration, in preparation for collecting a sample
00049 class ConfigManager:
00050     def __init__(self, cam_config, chain_config, laser_config, controller_config):
00051 
00052         # Set up Cameras
00053         rospy.logdebug("Constructing Cameras:")
00054         self._cam_managers = dict()
00055         for name in cam_config.keys():
00056             rospy.logdebug("  Constructing CamID [%s]" % name)
00057             self._cam_managers[name] = CameraConfigManager(name, cam_config[name])
00058 
00059         # Set up Chains
00060         rospy.logdebug("Constructing Chains:")
00061         self._chain_managers = dict()
00062         for name in chain_config.keys():
00063             rospy.logdebug("  Constructing ChainID [%s]" % name)
00064             self._chain_managers[name] = ChainConfigManager(name, chain_config[name])
00065 
00066         # Set up lasers
00067         rospy.logdebug("Constructing Lasers:")
00068         self._laser_managers = dict()
00069         for laser_id in laser_config.keys():
00070             rospy.logdebug("  Constructing LaserID [%s]" % laser_id)
00071             self._laser_managers[laser_id] = LaserConfigManager(laser_id, laser_config[laser_id])
00072 
00073         # Set up Controllers
00074         rospy.logdebug("Constructing Controllers:")
00075         self._controller_managers = dict()
00076         for controller_id in controller_config.keys():
00077             rospy.logdebug("  Constructing ControllerID [%s]" % controller_id)
00078             self._controller_managers[controller_id] = ControllerCmdManager(controller_id, controller_config[controller_id])
00079 
00080         # Set up interval_intersection
00081         self._intersect_ac = actionlib.SimpleActionClient("interval_intersection_config", interval_intersection.msg.ConfigAction)
00082 
00083     def reconfigure(self, config):
00084         # Reconfigure Interval Intersection
00085 
00086         intersect_goal = interval_intersection.msg.ConfigGoal();
00087         try: # handle case where there are no lasers
00088             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"]]
00089         except KeyError:
00090             intersect_goal.topics = [x['cam_id'] for x in config["camera_measurements"]] + [x['chain_id'] for x in config["joint_measurements"]]
00091         self._intersect_ac.send_goal(intersect_goal)
00092 
00093         # Reconfigure the cameras
00094         rospy.logdebug("Reconfiguring The Cameras")
00095         for cur_cam in config["camera_measurements"]:
00096             self._cam_managers[cur_cam["cam_id"]].reconfigure(cur_cam["config"])
00097 
00098         # Reconfigure the chains
00099         rospy.logdebug("Reconfiguring The Chains")
00100         for cur_chain in config["joint_measurements"]:
00101             self._chain_managers[cur_chain["chain_id"]].reconfigure(cur_chain["config"])
00102 
00103         # Reconfigure the lasers
00104         rospy.logdebug("Reconfiguring The Lasers")
00105         try:
00106             for cur_laser in config["laser_measurements"]:
00107                 self._laser_managers[cur_laser["laser_id"]].reconfigure(cur_laser["config"])
00108         except KeyError:
00109             pass # if no lasers, nothing to do.
00110 
00111         # Send controller commands
00112         rospy.logdebug("Sending Controller Commands")
00113         for cur_controller in config["joint_commands"]:
00114             self._controller_managers[cur_controller["controller"]].send_command(cur_controller)
00115 
00116     def stop(self):
00117         pass
00118 
00119 # Handles changing the configuration of a joint_states_settler
00120 class ChainConfigManager:
00121     def __init__(self, chain_id, configs):
00122         self._chain_id = chain_id
00123         self._configs = configs
00124         if not self.validate():
00125             raise Exception("Invalid chain description")
00126 
00127         # Initialize the ActionClient and state
00128         self._settler_ac  = actionlib.SimpleActionClient(self._configs["settler_config"], joint_states_settler.msg.ConfigAction)
00129         self._state = "idle"
00130 
00131     # Check to make sure that config dict has all the necessary fields. TODO: Currently a stub
00132     def validate(self):
00133         return True
00134 
00135     # Reconfigure this chain's processing pipeline to the specified configuration. Do nothing if
00136     # we're already in the correct configuration
00137     def reconfigure(self, next_config_name):
00138         if self._state == next_config_name:
00139             rospy.logdebug("  %s: Configured correctly as [%s]" % (self_.chain_id, self._state))
00140         else:
00141             rospy.logdebug("  %s: Need to transition [%s] -> [%s]" % (self._chain_id, self._state, next_config_name))
00142 
00143             next_config   = self._configs["configs"][next_config_name]
00144             settler_config = next_config["settler"]
00145             # Convert the settler's configuration from a dictionary to a ROS message
00146             goal = joint_states_settler.msg.ConfigGoal()
00147             goal.joint_names = settler_config["joint_names"]
00148             goal.tolerances  = settler_config["tolerances"]
00149             goal.max_step    = rospy.Duration(settler_config["max_step"])
00150             goal.cache_size  = settler_config["cache_size"]
00151 
00152             # Send the goal out
00153             self._settler_ac.send_goal(goal)
00154 
00155             # TODO: Need to add code that waits for goal to activate
00156 
00157 
00158 # Handles changing the configuration of the image pipeline associated with a camera
00159 class CameraConfigManager:
00160     def __init__(self, cam_id, configs):
00161         self._cam_id = cam_id
00162         self._configs = configs
00163         if not self.validate():
00164             raise Exception("Invalid camera description")
00165 
00166         # Initialize the ActionClients and state
00167         self._settler_ac   = actionlib.SimpleActionClient(self._configs["settler_config"], monocam_settler.msg.ConfigAction)
00168         self._led_detector_ac  = actionlib.SimpleActionClient(self._configs["cb_detector_config"], image_cb_detector.msg.ConfigAction)
00169         self._state = "idle"
00170 
00171     # Check to make sure that config dict has all the necessary fields. TODO: Currently a stub
00172     def validate(self):
00173         return True
00174 
00175     # Reconfigure this chain's processing pipeline to the specified configuration. Do nothing if
00176     # we're already in the correct configuration
00177     def reconfigure(self, next_config_name):
00178         if self._state == next_config_name:
00179             rospy.logdebug("  %s: Configured correctly as [%s]" % (self_.cam_id, self._state))
00180         else:
00181             rospy.logdebug("  %s: Need to transition [%s] -> [%s]" % (self._cam_id, self._state, next_config_name))
00182 
00183             next_config   = self._configs["configs"][next_config_name]
00184 
00185             # Send the Settler's Goal
00186             settler_config = next_config["settler"]
00187             goal = monocam_settler.msg.ConfigGoal()
00188             goal.tolerance = settler_config["tolerance"]
00189             goal.ignore_failures = settler_config["ignore_failures"]
00190             goal.max_step = rospy.Duration(settler_config["max_step"])
00191             goal.cache_size  = settler_config["cache_size"]
00192             self._settler_ac.send_goal(goal)
00193 
00194             # Send the CB Detector Goal
00195             cb_detector_config = next_config["cb_detector"]
00196             if not cb_detector_config["active"]:
00197                 rospy.logerr("Can't deal with inactive cb_detector")
00198             goal = image_cb_detector.msg.ConfigGoal()
00199             goal.num_x = cb_detector_config["num_x"]
00200             goal.num_y = cb_detector_config["num_y"]
00201             goal.spacing_x = 1.0    # Hardcoded garbage value. Should eventually be removed from the msg
00202             goal.spacing_y = 1.0    # Hardcoded garbage value. Should eventually be removed from the msg
00203             goal.width_scaling = cb_detector_config["width_scaling"]
00204             goal.height_scaling = cb_detector_config["height_scaling"]
00205             goal.subpixel_window = cb_detector_config["subpixel_window"]
00206             goal.subpixel_zero_zone = cb_detector_config["subpixel_zero_zone"]
00207             self._led_detector_ac.send_goal(goal)
00208 
00209             # Send the LED Detector Goal
00210             led_detector_config = next_config["led_detector"]
00211             if led_detector_config["active"]:
00212                 rospy.logerr("Can't deal with an active led_detector")
00213 
00214             # TODO: Need to add code that waits for goal to activate
00215 
00216 # Handles changing the configuration of the image pipeline associated with a camera
00217 class LaserConfigManager:
00218     def __init__(self, laser_id, configs):
00219         self._laser_id = laser_id
00220         self._configs = configs
00221         if not self.validate():
00222             raise Exception("Invalid laser description")
00223 
00224         # Initialize the ActionClients and state
00225         self._settler_ac   = actionlib.SimpleActionClient(self._configs["settler_config"], monocam_settler.msg.ConfigAction)
00226         self._cb_detector_ac  = actionlib.SimpleActionClient(self._configs["cb_detector_config"], laser_cb_detector.msg.ConfigAction)
00227         self._state = "idle"
00228 
00229     # Check to make sure that config dict has all the necessary fields. TODO: Currently a stub
00230     def validate(self):
00231         return True
00232 
00233     # Reconfigure this chain's processing pipeline to the specified configuration. Do nothing if
00234     # we're already in the correct configuration
00235     def reconfigure(self, next_config_name):
00236         if self._state == next_config_name:
00237             rospy.logdebug("  %s: Configured correctly as [%s]" % (self_.laser_id, self._state))
00238         else:
00239             rospy.logdebug("  %s: Need to transition [%s] -> [%s]" % (self._laser_id, self._state, next_config_name))
00240 
00241             next_config   = self._configs["configs"][next_config_name]
00242 
00243             # Send the Settler's Goal
00244             settler_config = next_config["settler"]
00245             goal = monocam_settler.msg.ConfigGoal()
00246             goal.tolerance = settler_config["tolerance"]
00247             goal.ignore_failures = settler_config["ignore_failures"]
00248             goal.max_step = rospy.Duration(settler_config["max_step"])
00249             goal.cache_size  = settler_config["cache_size"]
00250             self._settler_ac.send_goal(goal)
00251 
00252             # Send the CB Detector Goal
00253             cb_detector_config = next_config["cb_detector"]
00254             if not cb_detector_config["active"]:
00255                 rospy.logerr("Not sure yet how to deal with inactive cb_detector")
00256             goal = laser_cb_detector.msg.ConfigGoal()
00257             goal.num_x = cb_detector_config["num_x"]
00258             goal.num_y = cb_detector_config["num_y"]
00259             goal.spacing_x = cb_detector_config["spacing_x"]
00260             goal.spacing_y = cb_detector_config["spacing_y"]
00261             goal.width_scaling = cb_detector_config["width_scaling"]
00262             goal.height_scaling = cb_detector_config["height_scaling"]
00263             goal.subpixel_window = cb_detector_config["subpixel_window"]
00264             goal.subpixel_zero_zone = cb_detector_config["subpixel_zero_zone"]
00265             goal.flip_horizontal = cb_detector_config["flip_horizontal"]
00266             goal.min_intensity = cb_detector_config["min_intensity"]
00267             goal.max_intensity = cb_detector_config["max_intensity"]
00268             self._cb_detector_ac.send_goal(goal)
00269 
00270             # TODO: Need to add code that waits for goal to activate
00271 
00272 
00273 
00274 # Handles publishing commands to a trajectory controller
00275 class ControllerCmdManager:
00276     def __init__(self, controller_id, config):
00277         self._controller_id = controller_id
00278         self._config = config
00279         if not self.validate():
00280             raise Exception("Invalid chain description")
00281 
00282         # Initialize the Publisher
00283         self._pub  = rospy.Publisher(self._config["topic"], JointTrajectory)
00284 
00285     # Check to make sure that config dict has all the necessary fields. TODO: Currently a stub
00286     def validate(self):
00287         return True
00288 
00289     # Reconfigure this chain's processing pipeline to the specified configuration. Do nothing if
00290     # we're already in the correct configuration
00291     def send_command(self, cmd):
00292         rospy.logdebug("  Sending cmd to controller [%s]"%self._controller_id)
00293         cmd_msg = JointTrajectory()
00294         cmd_msg.header.stamp = rospy.Time().now()
00295         cmd_msg.joint_names = self._config["joint_names"]
00296         cmd_msg.points = [self._build_segment(x) for x in cmd["segments"]]
00297         self._pub.publish(cmd_msg)
00298 
00299     def _build_segment(self, config):
00300         segment = JointTrajectoryPoint()
00301         segment.positions = config["positions"]
00302         segment.velocities = [0] * len(config["positions"])
00303         segment.time_from_start = rospy.Duration(config["duration"])
00304         return segment
00305 
00306 
00307 


calibration_launch
Author(s): Michael Ferguson
autogenerated on Tue Sep 27 2016 04:06:51