def _update_move_base_params(self): """ If parameter updates have not been called in the last 5 seconds allow the subscriber callback to set them """ if ((rospy.Time.now().to_sec()-self.last_move_base_update) > 5.0): self.update_base_local_planner = True if self.update_base_local_planner: self.update_base_local_planner = False self.last_move_base_update = rospy.Time.now().to_sec() try: dyn_reconfigure_client= Client("/move_base/DWAPlannerROS",timeout=1.0) changes = dict() changes['acc_lim_x'] = maximum_f(self.valid_config.accel_limit_mps2, self.valid_config.decel_limit_mps2) changes['acc_lim_y'] = maximum_f(self.valid_config.accel_limit_mps2, self.valid_config.decel_limit_mps2) changes['acc_lim_th'] = self.valid_config.yaw_accel_limit_rps2 changes['max_vel_x'] = self.valid_config.x_vel_limit_mps changes['max_vel_y'] = self.valid_config.y_vel_limit_mps changes['max_rot_vel'] = self.valid_config.yaw_rate_limit_rps dyn_reconfigure_client.update_configuration(changes) dyn_reconfigure_client.close() except: pass rospy.loginfo("Vector Driver updated move_base parameters to match machine parameters")
def _update_move_base_params(self, config): """ If parameter updates have not been called in the last 5 seconds allow the subscriber callback to set them """ if ((rospy.Time.now().to_sec() - self.last_move_base_update) > 5.0): self.update_base_local_planner = True if self.update_base_local_planner: self.update_base_local_planner = False self.last_move_base_update = rospy.Time.now().to_sec() try: dyn_reconfigure_client = Client("/move_base/DWAPlannerROS", timeout=1.0) changes = dict() changes['acc_lim_x'] = maximum_f( self.valid_config.accel_limit_mps2, self.valid_config.decel_limit_mps2) changes['acc_lim_y'] = maximum_f( self.valid_config.accel_limit_mps2, self.valid_config.decel_limit_mps2) changes['acc_lim_th'] = self.valid_config.yaw_accel_limit_rps2 changes['max_vel_x'] = self.valid_config.x_vel_limit_mps changes['max_vel_y'] = self.valid_config.y_vel_limit_mps changes['max_rot_vel'] = self.valid_config.yaw_rate_limit_rps dyn_reconfigure_client.update_configuration(changes) dyn_reconfigure_client.close() except: pass rospy.loginfo( "Movo Driver updated move_base parameters to match machine parameters" )
def main(): # server_name = rospy.resolve_name('~data_collection_server') # root_dir = osp.expanduser(rospy.get_param('~root_dir')) server_name = '/data_collection_server' root_dir = osp.expanduser('~/.ros/instance_occlsegm') caller = rospy.ServiceProxy('%s/save_request' % server_name, Trigger) client = Client(server_name) action = 'n' # to initialize while not rospy.is_shutdown(): while action not in ['n', 's', 'q']: action = six.input( 'Please select actions: [n] New save_dir, [s] Save, [q] Quit: ' ) action = action.strip().lower() if action == 'n': now = rospy.Time.now() save_dir = osp.join(root_dir, str(now.to_nsec())) client.update_configuration({'save_dir': save_dir}) print('-' * 79) print('Updated save_dir to: {}'.format(save_dir)) elif action == 's': res = caller.call() print('Sent request to data_collection_server') print(res) else: assert action == 'q' return action = None
def configure_prosilica(): print "Configuring prosilica" client = ReconfigureClient("prosilica_driver") client.update_configuration({ 'auto_exposure': False, 'exposure': 0.4, 'auto_gain': False, 'gain': 0.0, 'auto_whitebalance': False, 'whitebalance_red': 114, 'whitebalance_blue': 300 })
def do_initial_config(self): # Do initial configuration. params = { "DRIVE_SPEED": State.DRIVE_SPEED, "REVERSE_SPEED": State.REVERSE_SPEED, "TURN_SPEED": State.TURN_SPEED, "HEADING_RESTORE_FACTOR": State.HEADING_RESTORE_FACTOR, "GOAL_DISTANCE_OK": State.GOAL_DISTANCE_OK, "ROTATE_THRESHOLD": State.ROTATE_THRESHOLD, "DRIVE_ANGLE_ABORT": State.DRIVE_ANGLE_ABORT, } dyn_client = Client(self.rover + '_MOBILITY') dyn_client.update_configuration(params) print('Initial configuration sent.')
class LaunchManager(object): def __init__(self): #self.launch = roslaunch.scriptapi.ROSLaunch() #self.launch.start() managed_launches = map(ManagedLaunch.factory, launches) self.managed_launches = dict([(ml.name, ml) for ml in managed_launches]) self.config = None self.srv = Server(GettingStartedConfig, self.dynparam_cb) self.client = Client("getting_started_node") def dynparam_cb(self, config, level): rospy.logdebug("Reconfigure Request: (%i) %s" % (level, (str(config)))) rospy.logdebug("keys: %s" % str(config.keys())) for name, ml in self.managed_launches.iteritems(): rospy.logdebug("cb: %s (%s)" % (name, str(ml))) if config[name]: args = [] if name.endswith("_map"): args = ["map_filename:=%s" % config.map_filename] rospy.loginfo("Map args: %s %s" % (name, args)) ml.start(args) else: ml.stop() self.config = config return config def spin_once(self): config_updates = {} for name, ml in self.managed_launches.iteritems(): rospy.logdebug("Spin: check if %s has died..." % name) if ml.died(): rospy.logwarn("Launch %s (%i) has died..." % (name, ml.process.pid)) ml.process = None config_updates[name] = False if config_updates: rospy.loginfo('updating config: %s' % str(config_updates)) #self.srv.update_configuration(config_updates) self.client.update_configuration(config_updates) def spin(self, rate=5.0): r = rospy.Rate(rate) while not rospy.is_shutdown(): self.spin_once() r.sleep()
def init_dyn_recfg(): cli = tcc.TwistControllerReconfigureClient() cli.init() cli.set_config_param(tcc.CTRL_IF, tcc.TwistController_VELOCITY_INTERFACE) cli.set_config_param(tcc.NUM_FILT, False) cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_MANIPULABILITY) cli.set_config_param(tcc.LAMBDA_MAX, 0.1) cli.set_config_param(tcc.W_THRESH, 0.05) # cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_LEAST_SINGULAR_VALUE) # cli.set_config_param(tcc.LAMBDA_MAX, 0.2) # cli.set_config_param(tcc.EPS_DAMP, 0.2) cli.set_config_param(tcc.EPS_TRUNC, 0.001) cli.set_config_param(tcc.PRIO_CA, 100) cli.set_config_param(tcc.PRIO_JLA, 50) cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS) cli.set_config_param(tcc.K_H, 1.0) cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA) cli.set_config_param(tcc.K_H_CA, -2.0) cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1) cli.set_config_param(tcc.ACTIV_BUF_CA, 50.0) cli.set_config_param(tcc.CRIT_THRESH_CA, 0.025) cli.set_config_param(tcc.DAMP_CA, 0.000001) cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA) cli.set_config_param(tcc.K_H_JLA, -1.0) cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0) cli.set_config_param(tcc.ACTIV_BUF_JLA, 300.0) cli.set_config_param(tcc.CRIT_THRESH_JLA, 5.0) cli.set_config_param(tcc.DAMP_JLA, 0.00001) cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION) cli.set_config_param(tcc.KEEP_DIR, True) cli.set_config_param(tcc.ENF_VEL_LIM, True) cli.set_config_param(tcc.ENF_POS_LIM, True) cli.update() cli.close() cli = Client('frame_tracker') ft_param = { 'cart_min_dist_threshold_lin': 0.2, 'cart_min_dist_threshold_rot': 0.2 } cli.update_configuration(ft_param) cli.close()
class Setup(smach.State): def __init__(self,max_window_size=75,step=5): smach.State.__init__(self, outcomes=['SETUP_DONE', 'FINISH'], input_keys=['counter_in','acc_results', 'cam_results', 'odom_results', 'imu_results', 'lidar_results', 'mic_results', 'result_cum', 'results_', 'x_array'], output_keys=['counter_out','acc_results', 'cam_results', 'odom_results', 'imu_results', 'lidar_results', 'mic_results', 'result_cum', 'results_', 'x_array']) #rospy.spin() self.step = step #self.acc_client = Client("accelerometer_process", timeout=3, config_callback=self.callback) self.cam_client = Client("vision_utils_ros_android", timeout=3, config_callback=self.callback) #self.odom_client = Client("odom_collisions", timeout=3, config_callback=self.callback) self.imu_client = Client("imu_collision_detection", timeout=3, config_callback=self.callback) self.lidar_client = Client("laser_collisions", timeout=3, config_callback=self.callback) self.mic_client = Client("mic_collisions", timeout=3, config_callback=self.callback) self.is_first_time = True rospy.sleep(0.2) def callback(self,config): #print (config) pass #rospy.loginfo("Config set to {double_param}, {int_param}, {double_param}, ".format(**config)) def execute(self, userdata): rospy.loginfo('Executing SETUP') #self.acc_client.update_configuration({"window_size": 10})#userdata.counter_in}) #self.acc_client.update_configuration({"reset": True}) #self.odom_client.update_configuration({"window_size": 10})#userdata.counter_in}) #self.odom_client.update_configuration({"reset": True}) self.imu_client.update_configuration({"window_size": 10})#userdata.counter_in}) #self.imu_client.update_configuration({"reset": True}) self.lidar_client.update_configuration({"window_size": 10})#userdata.counter_in}) #self.lidar_client.update_configuration({"reset": True}) self.mic_client.update_configuration({"window_size": 10}) #self.mic_client.update_configuration({"reset": True}) #SURF Version #self.cam_client.update_configuration({"matching_threshold": 100}) self.cam_client.update_configuration({"mode": 0 }) #self.cam_client.update_configuration({"reset": True}) rospy.sleep(0.5) if self.is_first_time:#userdata.counter_in < 75: # Window SIZe Define max TODO userdata.x_array.append(userdata.counter_in) userdata.counter_out = userdata.counter_in + self.step self.is_first_time = False return 'SETUP_DONE' else: userdata.results_['accel'] = userdata.acc_results userdata.results_['cam1'] = userdata.cam_results userdata.results_['odom'] = userdata.odom_results userdata.results_['imu'] = userdata.imu_results userdata.results_['lidar'] = userdata.lidar_results userdata.results_['mic'] = userdata.mic_results return 'FINISH'
def init_dyn_recfg(): cli = tcc.TwistControllerReconfigureClient() cli.init() cli.set_config_param(tcc.CTRL_IF, tcc.TwistController_VELOCITY_INTERFACE) cli.set_config_param(tcc.NUM_FILT, False) cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_CONSTANT) cli.set_config_param(tcc.DAMP_FACT, 0.2) cli.set_config_param(tcc.EPS_TRUNC, 0.001) cli.set_config_param(tcc.PRIO_CA, 100) cli.set_config_param(tcc.PRIO_JLA, 50) cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS) cli.set_config_param(tcc.K_H, 1.0) cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA) cli.set_config_param(tcc.K_H_CA, -1.0) cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1) cli.set_config_param(tcc.ACTIV_BUF_CA, 25.0) cli.set_config_param(tcc.CRIT_THRESH_CA, 0.025) cli.set_config_param(tcc.DAMP_CA, 0.000001) cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA) cli.set_config_param(tcc.K_H_JLA, -1.0) cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0) cli.set_config_param(tcc.ACTIV_BUF_JLA, 300.0) cli.set_config_param(tcc.CRIT_THRESH_JLA, 5.0) cli.set_config_param(tcc.DAMP_JLA, 0.00001) cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_BASE_ACTIVE) cli.set_config_param(tcc.EXT_RATIO, 0.05) cli.set_config_param(tcc.KEEP_DIR, True) cli.set_config_param(tcc.ENF_VEL_LIM, True) cli.set_config_param( tcc.ENF_POS_LIM, False) # To show that joint pos limits are violated if possible. cli.update() cli.close() cli = Client('frame_tracker') ft_param = { 'cart_min_dist_threshold_lin': 5.0, 'cart_min_dist_threshold_rot': 5.0 } cli.update_configuration(ft_param) cli.close()
class Client: def __init__(self, name, timeout=None): self.name = name self.client = DynamicReconfigureClient(name, timeout) def update(self, config): # send data out no greater than 10 times # revisit this and determine if this is actually needed c = 0 new_config = {} while c < 10: # fuerte: update_configuration returns the configuration in a different format # get new_config and compare with sent out config updated_config = self.client.update_configuration(config) for key,val in config.iteritems(): new_config[key] = updated_config[key] c += 1 if new_config == config: break if c == 10: pre = 'failed to set ' else: pre = 'successfully set ' sleep_time = 0.05 rospy.loginfo(pre + self.name + ' in ' + str(c) + ' attempt(s) and sleeping for ' + str(sleep_time) + 's') time.sleep(sleep_time)
def init_dyn_recfg(): cli = tcc.TwistControllerReconfigureClient() cli.init() cli.set_config_param(tcc.NUM_FILT, False) cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_MANIPULABILITY) cli.set_config_param(tcc.LAMBDA_MAX, 0.1) cli.set_config_param(tcc.W_THRESH, 0.05) cli.set_config_param(tcc.EPS_TRUNC, 0.001) cli.set_config_param(tcc.PRIO_CA, 50) cli.set_config_param( tcc.PRIO_JLA, 100) # change priorities now CA constraint has highest prio cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS) cli.set_config_param(tcc.K_H, 1.0) cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA) cli.set_config_param(tcc.K_H_CA, -1.0) cli.set_config_param(tcc.DAMP_CA, 0.000001) cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1) cli.set_config_param(tcc.ACTIV_BUF_CA, 50.0) cli.set_config_param(tcc.CRIT_THRESH_CA, 0.03) cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA) cli.set_config_param(tcc.K_H_JLA, -1.0) cli.set_config_param(tcc.DAMP_JLA, 0.000001) cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0) cli.set_config_param(tcc.ACTIV_BUF_JLA, 300.0) cli.set_config_param(tcc.CRIT_THRESH_JLA, 5.0) cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION) cli.set_config_param(tcc.KEEP_DIR, True) cli.set_config_param(tcc.ENF_VEL_LIM, True) cli.set_config_param(tcc.ENF_POS_LIM, True) cli.update() cli.close() cli = Client('frame_tracker') ft_param = { 'cart_min_dist_threshold_lin': 0.2, 'cart_min_dist_threshold_rot': 0.2 } cli.update_configuration(ft_param) cli.close()
class IntegrationTime: MIN = 30 MAX = 2000 def __init__(self): self.client = Client('camera/driver', timeout=1) def set(self, time): time = min(self.MAX, max(self.MIN, time)) self.client.update_configuration({'integration_time': time}) def get(self): return self.client.get_configuration()['integration_time'] def add(self, time): current = self.get() self.set(current + time)
def read_exposure(): time_avg = [] while not rospy.is_shutdown(): start = time.time() client_name = "ueye_cam_nodelet_front" client = Client(client_name) params = {"exposure": 33.0} client.update_configuration(params) rospy.get_param("/" + str(client_name) + "exposure", None) stop = time.time() duration = stop - start print(duration) time_avg.append(duration) print("Duration between call exposure API: ") time_avg = np.array(time_avg) print('max:', time_avg.max()) print('min:', time_avg.min()) print('mean:', time_avg.mean())
def init_dyn_recfg(): cli = tcc.TwistControllerReconfigureClient() cli.init() cli.set_config_param(tcc.CTRL_IF, tcc.TwistController_VELOCITY_INTERFACE) cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_MANIPULABILITY) cli.set_config_param(tcc.LAMBDA_MAX, 0.1) cli.set_config_param(tcc.W_THRESH, 0.05) cli.set_config_param(tcc.PRIO_CA, 100) cli.set_config_param(tcc.PRIO_JLA, 50) cli.set_config_param(tcc.SOLVER, tcc.TwistController_GPM) cli.set_config_param(tcc.K_H, 1.0) cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA) cli.set_config_param(tcc.K_H_CA, -2.0) cli.set_config_param(tcc.DAMP_CA, 0.000001) cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1) cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA) cli.set_config_param(tcc.K_H_JLA, -1.0) cli.set_config_param(tcc.DAMP_JLA, 0.00001) cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0) cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION) cli.set_config_param(tcc.KEEP_DIR, True) cli.set_config_param(tcc.ENF_VEL_LIM, True) cli.set_config_param( tcc.ENF_POS_LIM, False) # To show that joint pos limits are violated if possible. cli.update() cli.close() cli = Client('frame_tracker') ft_param = { 'cart_min_dist_threshold_lin': 0.2, 'cart_min_dist_threshold_rot': 0.2 } cli.update_configuration(ft_param) cli.close()
def __init__(self): self.__pub_rate = rospy.get_param('~rate', 10) #Hz self.__tf_broadcaster = tf2_ros.StaticTransformBroadcaster() self.__static_transformStamped = geometry_msgs.msg.TransformStamped() self.__srv = Server(TfConfig, self.__reconfigure_callback) self.__static_transformStamped.header.frame_id = rospy.get_param('~header_frame', "world") self.__static_transformStamped.child_frame_id = rospy.get_param('~child_frame', "base_link") self.__yaml_path = rospy.get_param('~yaml', "") if self.__yaml_path != "": try: yaml_file = open(self.__yaml_path, "r+") yaml_data = yaml.load(yaml_file) client = Client(rospy.get_name()) client.update_configuration(yaml_data) self.__set_tf(yaml_data) except IOError: rospy.logwarn('file not found in' + self.__yaml_path) rospy.logwarn("tf parameters are set 0.0") self.__set_zero() else: self.__set_zero()
class SpeechRecognitionClient(object): def __init__(self, srv_name="/speech_recognition", node_name="/speech_recognition", timeout=10): self._sr_srv = rospy.ServiceProxy(srv_name, SpeechRecognition) self._sr_srv.wait_for_service(timeout=timeout) self._cfg = Client(node_name, timeout=timeout) @property def language(self): return self._cfg.config["language"] @language.setter def language(self, value): self._cfg.update_configuration({'language': value}) @property def engine(self): return self._cfg.config["engine"] @engine.setter def engine(self, value): self._cfg.update_configuration({'engine': value}) @property def energy_threshold(self): return self._cfg.config["energy_threshold"] @energy_threshold.setter def energy_threshold(self, value): if self.dynamic_energy_threshold: rospy.logerr("Dynamic energy thresholding is enabled") else: self._cfg.update_configuration({ "energy_threshold": value }) @property def dynamic_energy_threshold(self): return self._cfg.config["dynamic_energy_threshold"] @dynamic_energy_threshold.setter def dynamic_energy_threshold(self, value): self._cfg.update_configuration({ "dynamic_energy_threshold": value }) def recognize(self, **args): return self._sr_srv(**args).result
def main(): rospy.init_node("follow_joint_trajectory_action_server") rospy.loginfo("%s started. Ctrl-c to stop." % rospy.get_name()) # Get ROS parameters joint_names = rospy.get_param('~joint_names', ['joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5']) ns = rospy.get_param('~namespace_of_topics', 'robot') dyn_cfg_srv = Server(PositionJointTrajectoryActionServerConfig, lambda config, level: config) # Reflect ROS parameters to dynamic reconfigure server client = Client(rospy.get_name()) client.update_configuration({('name_of_joint' + str(idx)):joint_name for idx, joint_name in enumerate(joint_names)}) fjtas = FollowJointTrajectoryActionServer(joint_names, ns, dyn_cfg_srv) def cleanup(): fjtas.clean_shutdown() rospy.on_shutdown(cleanup) rospy.spin()
class DriveTeleop: def __init__(self): self.clt = Client("drive_param_server", timeout=30, config_callback=self.param_server_callback) self.drive_state = 0 self.speed_setting = 2 # default to medium speed self.cmd_vel_pub = rospy.Publisher("teleop/cmd_vel", Twist, queue_size=10) self.joy_sub = rospy.Subscriber("joy", Joy, self.on_joy) def param_server_callback(self, config): self.drive_state = config['drive_state'] self.speed_setting = config['speed_setting'] rospy.loginfo(config) def on_joy(self, data): if data.buttons[6] == 1: self.drive_state = 1 - self.drive_state self.clt.update_configuration({"drive_state": self.drive_state}) if self.drive_state == 1: # Set speed ratio using d-pad if data.axes[7] == 1: # full speed (d-pad up) self.speed_setting = 1 self.clt.update_configuration({"speed_setting": 1}) if data.axes[6] != 0: # medium speed (d-pad left or right) self.speed_setting = 2 self.clt.update_configuration({"speed_setting": 2}) if data.axes[7] == -1: # low speed (d-pad down) self.speed_setting = 3 self.clt.update_configuration({"speed_setting": 3}) # Drive sticks left_speed = data.axes[1] / self.speed_setting # left stick right_speed = data.axes[4] / self.speed_setting # right stick # Convert skid steering speeds to twist speeds linear_vel = (left_speed + right_speed) / 2.0 # (m/s) angular_vel = (right_speed - left_speed) / 2.0 # (rad/s) # Publish Twist twist = Twist() twist.linear.x = linear_vel twist.angular.z = angular_vel self.cmd_vel_pub.publish(twist)
class SpeechRecognitionClient(object): def __init__(self, srv_name="/speech_recognition", node_name="/speech_recognition", timeout=10): self._sr_srv = rospy.ServiceProxy(srv_name, SpeechRecognition) self._sr_srv.wait_for_service(timeout=timeout) self._cfg = Client(node_name, timeout=timeout) @property def language(self): return self._cfg.config["language"] @language.setter def language(self, value): self._cfg.update_configuration({'language': value}) @property def engine(self): return self._cfg.config["engine"] @engine.setter def engine(self, value): self._cfg.update_configuration({'engine': value}) @property def energy_threshold(self): return self._cfg.config["energy_threshold"] @energy_threshold.setter def energy_threshold(self, value): if self.dynamic_energy_threshold: rospy.logerr("Dynamic energy thresholding is enabled") else: self._cfg.update_configuration({"energy_threshold": value}) @property def dynamic_energy_threshold(self): return self._cfg.config["dynamic_energy_threshold"] @dynamic_energy_threshold.setter def dynamic_energy_threshold(self, value): self._cfg.update_configuration({"dynamic_energy_threshold": value}) def recognize(self, **args): return self._sr_srv(**args).result
class CPPBridgeNode(ImageProcessor): def init_extended(self): self.cpp_process_service = rospy.get_param("~cpp_service", "cpp_process") self.multi_exposure = rospy.get_param("~multi_exposure", False) self.ignore_first_picture = True if self.multi_exposure: self.client = ReconfigureClient("prosilica_driver") self.client.update_configuration({'exposure': 0.4}) def get_second_image(self, image_topic): # Set exposure print "Changing the exposure of camera to 0.2" self.client.update_configuration({'exposure': 0.2}) rospy.sleep(0.1) image1 = TopicUtils.get_next_message(image_topic, Image) print "Changing the exposure of camera to 0.4" self.client.update_configuration({'exposure': 0.4}) rospy.sleep(0.1) image2 = TopicUtils.get_next_message(image_topic, Image) return (image1, image2) def process(self, cv_image, info, cv_image2=None): #ignore and get my own pictures if self.multi_exposure: (image, image2) = self.get_second_image("prosilica/image_rect_color") else: image = self.bridge.cv_to_imgmsg(cv_image, "bgr8") image2 = self.bridge.cv_to_imgmsg(cv_image, "bgr8") # save and display image in order to document the current experiment cvImage1 = self.bridge.imgmsg_to_cv(image, "bgr8") cvImage2 = self.bridge.imgmsg_to_cv(image2, "bgr8") timetag = strftime("%Y_%m_%d__%H_%M_%S") imageName1 = "/tmp/" + timetag + "_exposure_image1.png" imageName2 = "/tmp/" + timetag + "_exposure_image2.png" print "======= Storing images of 2 exposures =======" print "Saving image1: " + imageName1 cv.SaveImage(imageName1, cvImage1) print "Saving image2: " + imageName2 cv.SaveImage(imageName2, cvImage2) try: cpp_process = rospy.ServiceProxy(self.cpp_process_service, ProcessBridge) resp = cpp_process(image, info, image2) except rospy.ServiceException, e: rospy.loginfo("Service Call Failed: %s" % e) return ([], {}, cv_image) pts_y = resp.pts_y pts_x = resp.pts_x pts2d = [] for i in range(len(pts_y)): x = pts_x[i] y = pts_y[i] pts2d.append((x, y)) params_list = resp.params params = {} names = ("l", "r") for i, val in enumerate(params_list): params[names[i]] = val image_annotated = resp.image_annotated try: cv_image_annotated = self.bridge.imgmsg_to_cv( image_annotated, "bgr8") except CvBridgeError, e: "CVERROR converting from cv IplImage to ImageMessage"
class DynamicVelocityReconfigure(): "A class to reconfigure the velocity of the DWAPlannerROS." GAZE, NO_GAZE = range(2) def __init__(self, name): rospy.loginfo("Starting %s", name) self._action_name = name self.fast = True self.gaze_type = DynamicVelocityReconfigure.GAZE rospy.loginfo("Creating dynamic reconfigure client") self.client = DynClient("/move_base/DWAPlannerROS") rospy.loginfo(" ...done") rospy.loginfo("Creating base movement client.") self.baseClient = actionlib.SimpleActionClient( 'move_base', move_base_msgs.msg.MoveBaseAction) self.baseClient.wait_for_server() rospy.loginfo("Creating gaze client.") self.gazeClient = actionlib.SimpleActionClient( 'gaze_at_pose', strands_gazing.msg.GazeAtPoseAction) self.gazeClient.wait_for_server() rospy.loginfo("...done") # Magic numbers overwritten in dyn_callback self.threshold = 4.0 self.max_dist = 5.0 self.min_dist = 1.5 self.detection_angle = 90.0 self.dyn_srv = DynServer(HumanAwareNavigationConfig, self.dyn_callback) current_time = rospy.get_time() self.timeout = current_time + self.threshold rospy.loginfo("Creating action server.") self._as = actionlib.SimpleActionServer( self._action_name, move_base_msgs.msg.MoveBaseAction, self.goalCallback, auto_start=False) rospy.loginfo(" ...starting") self._as.start() rospy.loginfo(" ...done") self.sub_topic = rospy.get_param("~people_positions", '/people_tracker/positions') self.ppl_sub = None self.last_cancel_time = rospy.Time(0) rospy.Subscriber("/human_aware_navigation/cancel", actionlib_msgs.msg.GoalID, self.cancel_time_checker_cb) def dyn_callback(self, config, level): if config["gaze_type"] == DynamicVelocityReconfigure.NO_GAZE: self.cancel_gaze_goal() self.gaze_type = config["gaze_type"] self.threshold = config["timeout"] self.max_dist = config["max_dist"] self.min_dist = config["min_dist"] self.detection_angle = config["detection_angle"] max_vel_x = config["max_vel_x"] max_trans_vel = config["max_trans_vel"] max_rot_vel = config["max_rot_vel"] self.fast_param = { "max_vel_x": max_vel_x, "max_trans_vel": max_trans_vel, "max_rot_vel": max_rot_vel } return config def cancel_time_checker_cb(self, msg): self.last_cancel_time = rospy.get_rostime() def send_gaze_goal(self, topic): if self.gaze_type == DynamicVelocityReconfigure.GAZE: gaze_goal = strands_gazing.msg.GazeAtPoseGoal() gaze_goal.runtime_sec = 0 gaze_goal.topic_name = topic self.gazeClient.send_goal(gaze_goal) def cancel_gaze_goal(self): if self.gaze_type == DynamicVelocityReconfigure.GAZE: self.gazeClient.cancel_all_goals() def resetSpeed(self): rospy.logdebug("Resetting speeds to max:") rospy.logdebug(" Setting parameters: %s", self.fast_param) try: self.client.update_configuration(self.fast_param) except rospy.ServiceException as exc: rospy.logerr("Caught service exception: %s", exc) try: self.client.update_configuration(self.fast_param) except rospy.ServiceException as exc: rospy.logerr("Caught service exception: %s", exc) try: self.client.update_configuration(self.fast_param) except rospy.ServiceException as exc: rospy.logerr("Caught service exception: %s", exc) self.baseClient.cancel_all_goals() self.cancel_gaze_goal() self._as.set_aborted() def get_min_dist(self, data, angle): rad = angle * (np.pi / 180.0) distances = [ x for idx, x in enumerate(data.distances) if data.angles[idx] >= -rad and data.angles[idx] <= rad ] return np.min(distances) if len(distances) else 1000.0 def pedestrianCallback(self, pl): if not self._as.is_active(): rospy.logdebug("No active goal. Unsubscribing.") if self.ppl_sub: self.ppl_sub.unregister() self.ppl_sub = None return if len(pl.poses) > 0: rospy.logdebug("Found people: ") min_distance = self.get_min_dist(pl, self.detection_angle) rospy.logdebug(" People distance: %s", min_distance) factor = min_distance - self.min_dist factor = factor if factor > 0.0 else 0.0 factor /= (self.max_dist - self.min_dist) factor = 1.0 if factor > 1.0 else factor trans_speed = factor * self.fast_param['max_vel_x'] trans_speed = round(trans_speed, 2) rospy.logdebug("Calculated translational speed: %s", trans_speed) rot_speed = factor * self.fast_param['max_rot_vel'] rot_speed = round(rot_speed, 2) rospy.logdebug("Calculated rotaional speed: %s", rot_speed) if not trans_speed == self.fast_param[ 'max_vel_x']: # and not rot_speed == self.fast_param['max_rot_vel']: self.send_gaze_goal( "/upper_body_detector/closest_bounding_box_centre") self.slow_param = { 'max_vel_x': trans_speed, 'max_trans_vel': trans_speed, 'max_rot_vel': 0.0 if trans_speed < 0.05 else self.fast_param["max_rot_vel"] } try: print 'making it slow' self.client.update_configuration(self.slow_param) except rospy.ServiceException as exc: rospy.logerr("Caught service exception: %s", exc) rospy.logdebug(" Setting parameters: %s", self.slow_param) self.fast = False self.timeout = rospy.get_time() + self.threshold elif rospy.get_time() > self.timeout: rospy.logdebug("Not found any pedestrians:") if not self.fast: self.send_gaze_goal("/pose_extractor/pose") self.resetSpeed() self.fast = True else: rospy.logdebug(" Already fast") def goalCallback(self, goal): self.ppl_sub = rospy.Subscriber(self.sub_topic, PeopleTracker, self.pedestrianCallback, None, 1) self._goal = goal self.send_gaze_goal("/pose_extractor/pose") rospy.logdebug("Received goal:\n%s", self._goal) self.resetSpeed() self.moveBaseThread(self._goal) def preemptCallback(self): if rospy.get_rostime() - self.last_cancel_time < rospy.Duration(1): rospy.logdebug("Cancelled execution of goal:\n%s", self._goal) self.baseClient.cancel_all_goals() self.cancel_gaze_goal() self.resetSpeed() self._as.set_preempted() def moveBaseThread(self, goal): ret = self.moveBase(goal) self.resetSpeed() self.cancel_gaze_goal() if not self._as.is_preempt_requested() and ret: self._as.set_succeeded(self.result) elif not self._as.is_preempt_requested() and not ret: self._as.set_aborted(self.result) def moveBase(self, goal): rospy.logdebug('Moving robot to goal: %s', goal) self.baseClient.send_goal(goal, feedback_cb=self.moveBaseFeedbackCallback) status = self.baseClient.get_state() while status == actionlib_msgs.msg.GoalStatus.PENDING or status == actionlib_msgs.msg.GoalStatus.ACTIVE: status = self.baseClient.get_state() self.baseClient.wait_for_result(rospy.Duration(0.2)) self.result = self.baseClient.get_result() if self._as.is_preempt_requested(): self.preemptCallback() break if self.baseClient.get_state( ) != actionlib_msgs.msg.GoalStatus.SUCCEEDED and self.baseClient.get_state( ) != actionlib_msgs.msg.GoalStatus.PREEMPTED: return False #avoid jumping out of a state immediately after entering it - actionlib bug rospy.sleep(rospy.Duration.from_sec(0.3)) return True def moveBaseFeedbackCallback(self, fb): self._as.publish_feedback(fb)
def configure_prosilica(): print "Configuring prosilica" client = ReconfigureClient("prosilica_driver") client.update_configuration ({'auto_exposure':False,'exposure':0.4,'auto_gain':False,'gain':0.0, 'auto_whitebalance':False,'whitebalance_red':114,'whitebalance_blue':300})
class Camera: def __init__(self, node_name, proj, level, **paramnames): self.paramnames = paramnames self.name = node_name self.level = level self.proj = proj self.reconfigure_client = None self.async = AsynchronousUpdater(self.async_apply_update, "Camera "+node_name) self.trig_rising = True def param(self, config, name): return config[self.paramnames[name]] def setparam(self, config, name, value): config[self.paramnames[name]] = value # Uses the new configuration to compute new camera state. def process_update(self, config, level): # Produces: # MEMBERS # period, ext_trig, imager_period, end_offset # # PARAMETERS # "param_rate" self.trigger_name = "not_set" # Will be set by the CameraTriggerController # Took level checking out as it was causing problems with the projector # needed flag. #if self.level & level == 0: # return # This camera's configuration is unchanged. self.period = 1.0 / self.param(config, param_rate) projector_rate = config[param_proj_rate] self.reset_cameras = config["camera_reset"] self.ext_trig = True self.register_set = WGEConfig.WGE100Camera_PrimaryRegisterSet projector_limits_exposure = True trig_mode = self.param(config, param_trig_mode) # Internal triggering. if trig_mode == Config.CameraSynchronizer_InternalTrigger: self.ext_trig = False self.imager_period = self.period self.end_offset = -1 projector_limits_exposure = False else: if self.proj.mode == Config.CameraSynchronizer_ProjectorOff: trig_mode = Config.CameraSynchronizer_IgnoreProjector if trig_mode == Config.CameraSynchronizer_AlternateProjector or trig_mode == Config.CameraSynchronizer_WithProjector: self.proj.needed = True #print self.name, "setting needed to true." #else: #print self.name, "not setting needed to true." # Set the period if trig_mode == Config.CameraSynchronizer_AlternateProjector: n = round(self.period / self.proj.repeat_period - 0.5) n = max(n, 0) self.period = (n + 0.5) * self.proj.repeat_period #print "Case 1", n elif trig_mode == Config.CameraSynchronizer_IgnoreProjector: self.period = roundToEthercat(self.period) else: n = round(2 * self.period / self.proj.repeat_period - 1) n = max(n, 0) self.period = (n + 1) * self.proj.repeat_period / 2.0 #print "Case 2", n, self.period # Set the end_offset if trig_mode == Config.CameraSynchronizer_IgnoreProjector: self.end_offset = 0 projector_limits_exposure = False if trig_mode == Config.CameraSynchronizer_AlternateProjector: self.end_offset = self.proj.alt_end_offset self.register_set = WGEConfig.WGE100Camera_Auto elif trig_mode == Config.CameraSynchronizer_WithProjector: self.end_offset = self.proj.proj_end_offset self.register_set = WGEConfig.WGE100Camera_AlternateRegisterSet else: self.end_offset = self.proj.noproj_end_offset # Pick the imager period if trig_mode == Config.CameraSynchronizer_IgnoreProjector: self.imager_period = self.period - ETHERCAT_INTERVAL else: self.imager_period = self.proj.repeat_period / 2 - ETHERCAT_INTERVAL #print "Camera period", self.name, self.period, self.imager_period, self.proj.repeat_period if projector_limits_exposure: self.max_exposure = self.proj.noproj_max_exposure #print "Exposurei projector", self.max_exposure else: self.max_exposure = self.imager_period * 0.95 #print "Exposurei imager", self.max_exposure self.setparam(config, param_rate, 1/self.period) self.setparam(config, param_trig_mode, trig_mode) def async_apply_update(self, reconfig_request): try: #print "**** Start", self.name if self.reconfigure_client == None: #print "**** Making client", self.name self.reconfigure_client = DynamicReconfigureClient(self.name) #print "**** Made client", self.name self.reconfigure_client.update_configuration(reconfig_request) #print "**** Reconfigured client", self.name #print "Done updating camera ", self.name except KeyboardInterrupt: # Handle CTRL+C print "Aborted camera update on", self.name def apply_update(self): reconfig_request = { "ext_trig" : self.ext_trig, "trig_rate" : 1.0 / self.period, "imager_rate" : 1.0 / self.imager_period, "trig_timestamp_topic" : self.trigger_name, "rising_edge_trig" : self.trig_rising, "register_set" : self.register_set, "auto_exposure_alternate" : False, "exposure_alternate" : self.proj.proj_exposure_duration, "max_exposure" : self.max_exposure, } #print self.name, reconfig_request self.async.update(reconfig_request)
class WalkingInterfaceServer(object): def __init__(self, name): # Variables self._action_name = name self.display_no = rospy.get_param("~display", 0) self.start_time = 0 self.route = TopologicalRoute() self.coordinates = PoseArray() self.direction = "straight" self.previous_direction = "" self.head_pub = rospy.Publisher('/head/commanded_state', JointState, queue_size=10) self.head = JointState() self.head.header.stamp = rospy.Time.now() self.head.name = ["HeadPan", "HeadTilt"] self.topological_nodes = rospy.wait_for_message( '/topological_map', TopologicalMap) self.route_sub = rospy.Subscriber("/topological_navigation/Route", TopologicalRoute, self.route_callback) self.path_sub = message_filters.Subscriber("/move_base/NavfnROS/plan", Path) self.pose_sub = message_filters.Subscriber("/pose_extractor/pose", PoseStamped) self.ts = message_filters.ApproximateTimeSynchronizer( [self.path_sub, self.pose_sub], 10, 10) self.ts.registerCallback(self.filter_callback) self.dyn_client = DynClient('/EBC') self.thread = None #tell the webserver where it should look for web files to serve #http_root = os.path.join( # roslib.packages.get_pkg_dir("aaf_walking_group"), # "www") #client_utils.set_http_root(http_root) #Starting server rospy.loginfo("%s: Starting walking interface action server", name) self._as = actionlib.SimpleActionServer( self._action_name, EmptyAction, execute_cb=self.executeCallback, auto_start=False) self._as.start() rospy.loginfo("%s: ...done.", name) def filter_callback(self, path, pose): self.start_time = time.time() robot_angle = tf.transformations.euler_from_quaternion([ pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w ]) yDiff = path.poses[-1].pose.position.y - pose.pose.position.y xDiff = path.poses[-1].pose.position.x - pose.pose.position.x dist = math.sqrt(xDiff * xDiff + yDiff * yDiff) if dist > 1.0: angle = robot_angle[-1] - math.atan2(yDiff, xDiff) while angle > math.pi: angle -= 2 * math.pi while angle < -math.pi: angle += 2 * math.pi #print math.degrees(angle) if abs(math.degrees(angle)) < 30: self.direction = "straight" else: if angle > 0: self.direction = "right" else: self.direction = "left" else: self.direction = "straight" def route_callback(self, data): #search for waypouint and store coordinates print data.nodes for i in data.nodes: for j in self.topological_nodes.nodes: if i == j.name: self.coordinates.poses.append(j.pose) def executeCallback(self, goal): self.previous_direction = "" while not self._as.is_preempt_requested() and not rospy.is_shutdown(): if time.time() - self.start_time > 5: #reconfigurable parameter self.direction = "stop" if self.previous_direction != self.direction: client_utils.display_relative_page(self.display_no, 'stop.html') rospy.loginfo("STOP") self.indicate = False #move head straight self.head.position = [0, 0] self.head_pub.publish(self.head) self.previous_direction = "stop" else: if self.previous_direction != self.direction: if self.direction == 'right': client_utils.display_relative_page( self.display_no, 'turn_right.html') #move head right self.head.position = [-30, 0] self.head_pub.publish(self.head) #indicate if self.thread: self.indicate = False self.thread.join() self.indicate = True self.thread = Thread(target=self.blink, args=('right', )) self.thread.start() rospy.loginfo("Moving right...") self.previous_direction = "right" elif self.direction == 'left': client_utils.display_relative_page( self.display_no, 'turn_left.html') #move head left self.head.position = [30, 0] self.head_pub.publish(self.head) #indicate if self.thread: self.indicate = False self.thread.join() self.indicate = True self.thread = Thread(target=self.blink, args=('left', )) self.thread.start() rospy.loginfo("Moving left...") self.previous_direction = "left" else: client_utils.display_relative_page( self.display_no, 'straight.html') self.indicate = False #move head straight self.head.position = [0, 0] self.head_pub.publish(self.head) rospy.loginfo("Moving straight...") self.previous_direction = "straight" if self._as.is_preempt_requested(): self._as.set_preempted() else: self._as.set_succeeded() def switchIndicator(self, isOn, side): if side == "left": params = {'Port0_5V_Enabled': isOn} else: params = {'Port1_5V_Enabled': isOn} self.dyn_client.update_configuration(params) def blink(self, side): r = rospy.Rate(2) toggle = True while self.indicate and not rospy.is_shutdown(): self.switchIndicator(toggle, side) toggle = not toggle r.sleep() self.switchIndicator(False, side) def _on_node_shutdown(self): self.client.cancel_all_goals()
class CPPBridgeNode(ImageProcessor): def init_extended(self): self.cpp_process_service = rospy.get_param("~cpp_service","cpp_process") self.multi_exposure = rospy.get_param("~multi_exposure",False) self.ignore_first_picture = True if self.multi_exposure: self.client = ReconfigureClient("prosilica_driver") self.client.update_configuration({'exposure':0.4}) def get_second_image(self,image_topic): # Set exposure print "Changing the exposure of camera to 0.2" self.client.update_configuration({'exposure':0.2}) rospy.sleep(0.1) image1 = TopicUtils.get_next_message(image_topic,Image) print "Changing the exposure of camera to 0.4" self.client.update_configuration({'exposure':0.4}) rospy.sleep(0.1) image2 = TopicUtils.get_next_message(image_topic,Image) return (image1,image2) def process(self,cv_image,info,cv_image2=None): #ignore and get my own pictures if self.multi_exposure: (image,image2) = self.get_second_image("prosilica/image_rect_color") else: image = self.bridge.cv_to_imgmsg(cv_image,"bgr8") image2 = self.bridge.cv_to_imgmsg(cv_image,"bgr8") # save and display image in order to document the current experiment cvImage1= self.bridge.imgmsg_to_cv(image, "bgr8") cvImage2= self.bridge.imgmsg_to_cv(image2, "bgr8") timetag= strftime("%Y_%m_%d__%H_%M_%S") imageName1= "/tmp/" + timetag + "_exposure_image1.png" imageName2= "/tmp/" + timetag + "_exposure_image2.png" print "======= Storing images of 2 exposures =======" print "Saving image1: " + imageName1 cv.SaveImage(imageName1, cvImage1); print "Saving image2: " + imageName2 cv.SaveImage(imageName2, cvImage2); try: cpp_process = rospy.ServiceProxy(self.cpp_process_service,ProcessBridge) resp = cpp_process(image,info,image2) except rospy.ServiceException,e: rospy.loginfo("Service Call Failed: %s"%e) return ([],{},cv_image) pts_y = resp.pts_y pts_x = resp.pts_x pts2d = [] for i in range(len(pts_y)): x = pts_x[i] y = pts_y[i] pts2d.append((x,y)) params_list = resp.params params = {} names = ("l","r") for i,val in enumerate(params_list): params[names[i]] = val image_annotated = resp.image_annotated try: cv_image_annotated = self.bridge.imgmsg_to_cv(image_annotated, "bgr8") except CvBridgeError, e: "CVERROR converting from cv IplImage to ImageMessage"
class WalkingGroupStateMachine(object): def __init__(self, name): rospy.loginfo("Creating " + name + " server...") self._as = actionlib.SimpleActionServer( name, StateMachineAction, self.execute, auto_start=False ) self._as.register_preempt_callback(self.preempt_callback) rospy.loginfo("Creating guiding client...") nav_client = actionlib.SimpleActionClient("guiding", GuidingAction) nav_client.wait_for_server() rospy.loginfo(" ... done") rospy.loginfo("Creating wait client...") wait_client = actionlib.SimpleActionClient("wait_for_participant", EmptyAction) wait_client.wait_for_server() rospy.loginfo(" ... done") rospy.loginfo("Waiting for recovery toggle services...") rospy.loginfo(" ... images") self.rec_srv = rospy.ServiceProxy('/toggle_walking_group_recovery', ToggleWalkingGroupRecovery) self.rec_srv.wait_for_service() rospy.loginfo(" ... done") rospy.loginfo("Waiting for media server services...") rospy.loginfo(" ... images") s = rospy.ServiceProxy('/aaf_walking_group/image_server/get_id', GetMediaId) s.wait_for_service() rospy.loginfo(" ... video") s = rospy.ServiceProxy('/aaf_walking_group/video_server/get_id', GetMediaId) s.wait_for_service() rospy.loginfo(" ... sound") rospy.loginfo(" ... music") s = rospy.ServiceProxy('/music_player_service', MusicPlayerService) s.wait_for_service() rospy.loginfo(" ... jingles") s = rospy.ServiceProxy('/sound_player_service', PlaySoundService) s.wait_for_service() rospy.loginfo(" ... waypoints") s = rospy.ServiceProxy('/aaf_waypoint_sounds_service', WaypointSoundsService) s.wait_for_service() rospy.loginfo(" ... done") # Get parameters self.display_no = rospy.get_param("~display_no", 0) self.waypointset_name = rospy.get_param("~mongodb_params/waypointset_name", "") self.waypointset_collection = rospy.get_param("~mongodb_params/waypointset_collection", "aaf_walking_group") self.waypointset_meta = rospy.get_param("~mongodb_params/waypointset_meta", "waypoint_set") if self.waypointset_name == "": rospy.logfatal("Missing parameters.") rospy.logfatal("Please run with _mongodb_params/waypointset_name:=<waypointset_name>") return self.preempt_srv = None self.ptu = PTU() self.gaze = Gaze() self.recovery = RecoveryReconfigure( name="/monitored_navigation/recover_states/", whitelist=rosparam.load_file(rospy.get_param("~recovery_whitelist"))[0][0]["recover_states"] ) self.dyn_client = DynClient( "/human_aware_navigation" ) self.get_current_han_settings() rospy.loginfo(" ... starting " + name) self._as.start() rospy.loginfo(" ... started " + name) def get_current_han_settings(self): gazing = rospy.get_param("/human_aware_navigation/gaze_type") angle = round(rospy.get_param("/human_aware_navigation/detection_angle"),2) self.han_param = { 'gaze_type': gazing, 'detection_angle': angle } rospy.loginfo("Found following default values for human_aware_navigation: %s", self.han_param) def execute(self, goal): rospy.loginfo("Starting state machine") try: rospy.loginfo("Creating recovery toggle service proxy and waiting ...") self.rec_srv.wait_for_service() rospy.loginfo(" .. calling recovery toggle service") self.rec_srv(True) rospy.loginfo(" .. started walking group recovery") except (rospy.ServiceException, rospy.ROSInterruptException) as e: rospy.logwarn("waypoint sound service call failed: %s" % e) #Save current recovery behaviour states self.recovery.save_current_configuration() #and reconfigure self.recovery.reconfigure(RecoveryReconfigure.SET) # Setting http root http_root = roslib.packages.get_pkg_dir('aaf_walking_group') + '/www' strands_webserver.client_utils.set_http_root(http_root) self.ptu.turnPTU(-180, 10) dyn_param = { 'timeout': 0.0, 'gaze_type': 1, 'detection_angle': 80.0 } try: self.dyn_client.update_configuration(dyn_param) except rospy.ServiceException as e: rospy.logerr("Caught service exception: %s", e) self.preempt_srv = rospy.Service('/walking_group/cancel', Empty, self.preempt_srv_cb) self.waypointset = self.loadConfig(self.waypointset_name, collection_name=self.waypointset_collection, meta_name=self.waypointset_meta) pprint.pprint(self.waypointset) try: rospy.loginfo("Creating waypoint sound service proxy and waiting ...") s = rospy.ServiceProxy('aaf_waypoint_sounds_service', WaypointSoundsService) s.wait_for_service() rospy.loginfo(" .. calling waypoint sound service") s(WaypointSoundsServiceRequest.RESUME) rospy.loginfo(" .. started waypoint sound service") except (rospy.ServiceException, rospy.ROSInterruptException) as e: rospy.logwarn("waypoint sound service call failed: %s" % e) # Create a SMACH state machine self.sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) self.sm.userdata.current_waypoint = self.waypointset[goal.group]["waypoints"][str(min([int(x) for x in self.waypointset[goal.group]["waypoints"].keys()]))] self.sm.userdata.play_music = True sis = smach_ros.IntrospectionServer( 'walking_group_state_machine', self.sm, '/walking_group_machine' ) sis.start() # Open the container with self.sm: # Add states to the container smach.StateMachine.add( 'ENTERTAIN', Entertain(self.display_no, self.gaze), transitions={ 'key_card': 'GUIDE_INTERFACE', 'killall': 'preempted' }, remapping={'current_waypoint' : 'current_waypoint', 'play_music' : 'play_music'} ) smach.StateMachine.add( 'GUIDE_INTERFACE', GuideInterface(self.waypointset[goal.group]["waypoints"]), transitions={ 'move_to_point': 'GUIDING', 'aborted': 'ENTERTAIN', 'killall': 'preempted' }, remapping={'current_waypoint' : 'current_waypoint', 'play_music' : 'play_music'} ) smach.StateMachine.add( 'GUIDING', Guiding(waypoints=self.waypointset[goal.group]["waypoints"], distance=self.waypointset[goal.group]["stopping_distance"]), transitions={ 'reached_point': 'RESTING_CONT', 'reached_final_point': 'succeeded', 'key_card': 'GUIDE_INTERFACE', 'killall': 'preempted' }, remapping={'waypoint' : 'waypoint', 'play_music' : 'play_music'} ) smach.StateMachine.add( 'RESTING_CONT', RestingPoint(self.display_no,self.waypointset[goal.group]["waypoints"]), transitions={ 'rest': 'ENTERTAIN', 'continue': 'GUIDING', 'key_card': 'GUIDE_INTERFACE', 'killall': 'preempted' }, remapping={'current_waypoint' : 'current_waypoint', 'play_music' : 'play_music'} ) # Execute SMACH plan self.sm.execute() sis.stop() self.preempt_srv.shutdown() self.ptu.turnPTU(0, 0) self.recovery.reconfigure(RecoveryReconfigure.RESET) try: self.dyn_client.update_configuration(self.han_param) except rospy.ServiceException as e: rospy.logerr("Caught service exception: %s", e) try: rospy.loginfo("Creating waypoint sound service proxy and waiting ...") s = rospy.ServiceProxy('aaf_waypoint_sounds_service', WaypointSoundsService) s.wait_for_service() rospy.loginfo(" ... calling waypoint sound service") s(WaypointSoundsServiceRequest.PAUSE) rospy.loginfo(" ... stopped waypoint sound service") except (rospy.ServiceException, rospy.ROSInterruptException) as e: rospy.logwarn("Service call failed: %s" % e) try: rospy.loginfo("Creating recovery toggle service proxy and waiting ...") self.rec_srv.wait_for_service() rospy.loginfo(" .. calling recovery toggle service") self.rec_srv(False) rospy.loginfo(" .. stopped walking group recovery") except (rospy.ServiceException, rospy.ROSInterruptException) as e: rospy.logwarn("Service call failed: %s" % e) if not self._as.is_preempt_requested() and self._as.is_active(): self._as.set_succeeded() else: self._as.set_preempted() def preempt_callback(self): rospy.logwarn("Walking group preempt requested") self.sm.request_preempt() def preempt_srv_cb(self, req): self.preempt_callback() return EmptyResponse() def loadConfig(self, dataset_name, collection_name="aaf_walking_group", meta_name="waypoint_set"): msg_store = MessageStoreProxy(collection=collection_name) query_meta = {} query_meta[meta_name] = dataset_name if len(msg_store.query(std_msgs.msg.String._type, {}, query_meta)) == 0: rospy.logerr("Desired data set '"+meta_name+": "+dataset_name+"' not in datacentre.") raise Exception("Can't find data in datacentre.") else: message = msg_store.query(std_msgs.msg.String._type, {}, query_meta) return json.loads(message[0][0].data)
class DynamicConfigurationParameters: def __init__(self): self.defaults = {} for param_name in rospy.get_param_names(): if not param_name.startswith(rospy.get_name()): continue value = str(rospy.get_param(param_name)) index = value.find(';') name = value[:index] path = value[index+1:] param_name = param_name.split('/')[2] self.defaults[param_name] = { 'name': name, 'path': path, } rospy.Service('/mercury/dynparam/defaults', Default, self.default_handler) rospy.Service('/mercury/dynparam/load', Load, self.load_handler) def load(self, name, path): if not os.path.exists(path): return False if not name.startswith('/'): name = '/' + name f = file(path, 'r') try: params = {} for doc in yaml.load_all(f.read(), Loader=yaml.FullLoader): params.update(doc) finally: f.close() try: self.client = DynamicReconfigureClient(name) try: self.client.update_configuration(params) except DynamicReconfigureCallbackException as err: logger.log_error(str(err)) return False except DynamicReconfigureParameterException as err: logger.log_error(str(err)) return False return True finally: self.client.close() def default_handler(self, request): key = request.name if key not in self.defaults: return False logger.log_warn("loading default parameters for %s ...", key) return self.load(self.defaults[key]['name'], self.defaults[key]['path']) def load_handler(self, request): if request.name == "": return False return self.load(request.name, request.path) def spin(self): rospy.spin()
# returns. rospy.init_node('testclient_py', anonymous=True) client = DynamicReconfigureClient('/dynamic_reconfigure_test_server', config_callback=config_callback, timeout=5) time.sleep(1) # You can also get the configuration manually by calling get_configuration. print("Configuration from get_configuration:") print_config(client.get_configuration(timeout=5)) time.sleep(1) # You can push changes to the server using the update_configuration method. # You can set any subset of the node's parameters using this method. It # returns out the full new configuration of the server (which may differ # from what you requested if you asked for something illegal). print("Configuration after setting int_ to 4:") print_config(client.update_configuration({'int_': 4})) time.sleep(1) print("Configuration after setting int_ to 0 and bool_ to True:") print_config(client.update_configuration({'int_': 0, 'bool_': True})) time.sleep(1) # You can access constants defined in Test.cfg file in the following way: import dynamic_reconfigure.cfg.TestConfig as Config print("Medium is a constant that is set to 1:", Config.Test_Medium) # This is useful for setting enums: print("Configuration after setting int_enum_ to Medium:") print_config(client.update_configuration({'int_enum_': Config.Test_Medium})) time.sleep(1)
class MoveToFixedWaypoint(EventState): ''' Lets the robot move to a given waypoint. -- allow_backwards Boolean Allow the robot to drive backwards when approaching the given waypoint. ># waypoint PoseStamped Specifies the waypoint to which the robot should move. ># speed Speed of the robot <= reached Robot is now located at the specified waypoint. <= failed Failed to send a motion request to the action server. ''' def __init__(self, allow_backwards = False): ''' Constructor ''' super(MoveToFixedWaypoint, self).__init__(outcomes=['reached', 'failed'], input_keys=['waypoint','speed']) self._action_topic = '/move_base' self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction}) #self.set_tolerance = rospy.ServiceProxy('/controller/set_alternative_tolerances', SetAlternativeTolerance) self._dynrec = Client("/vehicle_controller", timeout = 10) self._defaultspeed = 0.1 self._allow_backwards = allow_backwards self._failed = False self._reached = False def execute(self, userdata): ''' Execute this state ''' if self._failed: return 'failed' if self._reached: return 'reached' if self._move_client.has_result(self._action_topic): result = self._move_client.get_result(self._action_topic) self._dynrec.update_configuration({'speed':self._defaultspeed}) if result.result == 1: self._reached = True return 'reached' else: self._failed = True Logger.logwarn('Failed to reach waypoint!') return 'failed' def on_enter(self, userdata): speedValue = self._dynrec.get_configuration(timeout = 0.5) if speedValue is not None: self._defaultspeed = speedValue['speed'] self._dynrec.update_configuration({'speed':userdata.speed}) self._startTime = Time.now() self._failed = False self._reached = False #goal_id = GoalID() #goal_id.id = 'abcd' #goal_id.stamp = Time.now() action_goal = MoveBaseGoal() action_goal.target_pose = userdata.waypoint action_goal.speed = userdata.speed action_goal.reverse_allowed = self._allow_backwards if action_goal.target_pose.header.frame_id == "": action_goal.target_pose.header.frame_id = "world" try: self._move_client.send_goal(self._action_topic, action_goal) #resp = self.set_tolerance(goal_id, 0.2, 1.55) except Exception as e: Logger.logwarn('Failed to send motion request to waypoint (%(x).3f, %(y).3f):\n%(err)s' % { 'err': str(e), 'x': userdata.waypoint.pose.position.x, 'y': userdata.waypoint.pose.position.y }) self._failed = True Logger.loginfo('Driving to next waypoint') def on_stop(self): try: if self._move_client.is_available(self._action_topic) \ and not self._move_client.has_result(self._action_topic): self._move_client.cancel(self._action_topic) except: # client already closed pass def on_exit(self, userdata): try: if self._move_client.is_available(self._action_topic) \ and not self._move_client.has_result(self._action_topic): self._move_client.cancel(self._action_topic) except: # client already closed pass def on_pause(self): self._move_client.cancel(self._action_topic) def on_resume(self, userdata): self.on_enter(userdata)
class DoUTurnAndGoToStop1Stop2ThenSAndHomeWhileStopping(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['succeeded', 'failed']) self.last_pose = None self.ndt_pose_sub = rospy.Subscriber('/ndt_pose', PoseStamped, self.ndt_pose_cb, queue_size=1) self.twist_gain_dyn_client = Client('/twist_gains/twist_gains') def _stop_sign_detected_cb(self, msg): self.last_stop = msg.data def ndt_pose_cb(self, msg): self.last_pose = msg def did_we_see_stop(self): msg = rospy.wait_for_message('/stop_detection_status', String, 4.0) if msg.data == 'STOP': return True else: return False def are_we_at_stop_1(self): stop_1_x = -57.978 stop_1_y = 112.837 return self.are_we_at(stop_1_x, stop_1_y) def are_we_at_stop_2(self): stop_1_x = -53.440 stop_1_y = 57.763 return self.are_we_at(stop_1_x, stop_1_y) def are_we_at(self, x, y, tolerance=0.5): curr_x = self.last_pose.pose.position.x curr_y = self.last_pose.pose.position.y dist = math.hypot(curr_x - x, curr_y - y) rospy.loginfo("We are at " + str(round(dist, 2)) + "m") if abs(dist) < tolerance: return True else: return False def stop_car_3s(self): ini_t = time.time() rospy.logerr("======== STOPPING FOR 3S ===========") self.twist_gain_dyn_client.update_configuration({ 'linear_x_gain': 0.0, 'angular_z_gain': 0.0 }) while not rospy.is_shutdown() and (time.time() - ini_t) < 5.0: rospy.sleep(0.05) rospy.loginfo("Done!") self.twist_gain_dyn_client.update_configuration({ 'linear_x_gain': 1.0, 'angular_z_gain': 25.0 }) def execute(self, userdata): rospy.loginfo('Executing state ' + self.__class__.__name__) # Send a goal to our "Move using waypoints" server and wait until # we reach the goal fwf = FollowWaypointsFile('mission_5_6_7.csv', consider_done_on_waypoint_id=489) rospy.sleep(2.0) # keep checking for our pose to be in the radius of the first stop signal while not rospy.is_shutdown() and \ not self.are_we_at_stop_1(): rospy.sleep(0.1) if self.did_we_see_stop(): # stop the car 3s self.stop_car_3s() # wait for the car to move enough to stop seeing the STOP sign and reset rospy.sleep(10.0) self.last_stop = 'GO' # keep checking for our pose to be in the radius of the first stop signal while not rospy.is_shutdown() and \ not self.are_we_at_stop_2(): rospy.sleep(0.1) if self.did_we_see_stop(): # stop the car 3s self.stop_car_3s() fwf.wait_to_reach_last_waypoint() return 'succeeded'
def people_cb2(msg): global tx2_people tx2_people = msg if __name__ == '__main__': rospy.init_node('visualize_openpose') stage = rospy.get_param('~stage', 1) compute_openpose_xavier = rospy.ServiceProxy('compute_openpose', Compute) compute_openpose_xavier.wait_for_service() reconf_xavier = ReconfClient('openpose_xavier') reconf_xavier.update_configuration({ 'key_point_threshold': 0.1, 'affinity_threshold': 0.1, 'line_division': 5 }) reconf_tx2 = ReconfClient('openpose_tx2') reconf_tx2.update_configuration({ 'key_point_threshold': 0.1, 'affinity_threshold': 0.1, 'line_division': 2 }) #st_sub = rospy.Subscriber('openpose_stage1', SparseTensorArray, # sp_cb, queue_size=1) rospy.Subscriber('image', Image, img_cb, queue_size=1,
client = DynamicReconfigureClient('/dynamic_reconfigure_test_server', config_callback=config_callback, timeout=5) time.sleep(1) # You can also get the configuration manually by calling get_configuration. print("Configuration from get_configuration:") print_config(client.get_configuration(timeout=5)) time.sleep(1) # You can push changes to the server using the update_configuration method. # You can set any subset of the node's parameters using this method. It # returns out the full new configuration of the server (which may differ # from what you requested if you asked for something illegal). print("Configuration after setting int_ to 4:") print_config(client.update_configuration({'int_': 4})) time.sleep(1) print("Configuration after setting int_ to 0 and bool_ to True:") print_config(client.update_configuration({'int_': 0, 'bool_': True})) time.sleep(1) # You can access constants defined in Test.cfg file in the following way: import dynamic_reconfigure.cfg.TestConfig as Config print("Medium is a constant that is set to 1:", Config.Test_Medium) # This is useful for setting enums: print("Configuration after setting int_enum_ to Medium:") print_config(client.update_configuration({'int_enum_': Config.Test_Medium})) time.sleep(1)
def __init__(self, robot, rack_id): StateMachine.__init__(self, outcomes=['done']) public_arm = robot.get_arm() arm = public_arm._arm local_client = Client("/hero/local_planner/local_costmap") local_client.update_configuration({"footprint": []}) global_client = Client("/hero/global_planner/global_costmap") global_client.update_configuration({"footprint": []}) def send_joint_goal(position_array, wait_for_motion_done=True): arm._send_joint_trajectory([position_array], timeout=rospy.Duration(0)) if wait_for_motion_done: arm.wait_for_motion_done() def send_gripper_goal(open_close_string, max_torque=1.0): arm.send_gripper_goal(open_close_string, max_torque=max_torque) rospy.sleep(1.0) # Does not work with motion_done apparently def show_image(package_name, path_to_image_in_package): path = os.path.join(rospkg.RosPack().get_path(package_name), path_to_image_in_package) if not os.path.exists(path): rospy.logerr("Image path {} does not exist".format(path)) else: try: rospy.loginfo("Showing {}".format(path)) robot.hmi.show_image(path, 10) except Exception as e: rospy.logerr("Could not show image {}: {}".format(path, e)) return 'succeeded' @cb_interface(outcomes=['done']) def _pre_grab(_): robot.speech.speak("Hey, I found myself a nice rack") send_joint_goal([0, 0, 0, 0, 0]) send_joint_goal([0.69, 0, 0, 0, 0]) rospy.sleep(1.0) send_gripper_goal("open") send_joint_goal([0.69, -1.77, 0, -1.37, 1.57]) return 'done' @cb_interface(outcomes=['done']) def _align(_): robot.head.look_down() robot.speech.speak("Let's see what I can do with this") goal_pose = PoseStamped() goal_pose.header.stamp = rospy.Time.now() goal_pose.header.frame_id = rack_id goal_pose.pose.orientation = Quaternion( *quaternion_from_euler(0, 0, math.pi)) goal_pose.pose.position.x = 0.45 goal_pose.pose.position.y = 0.09 ControlToPose( robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.02, 0.05)).execute({}) return 'done' @cb_interface(outcomes=['done']) def _grab(_): robot.speech.speak("Grasping the trolley") rospy.sleep(4.0) send_joint_goal([0.61, -1.77, 0, -1.37, 1.57]) send_gripper_goal("close") base_footprint = [[0.491716563702, 0.284912616014], [0.504091262817, -0.264433205128], [0.00334876775742, -0.259195685387], [-0.17166364193, -0.19022783637], [-0.239429235458, -0.07719720155], [-0.237978458405, 0.0547728165984], [-0.180378556252, 0.164403796196], [-0.0865250825882, 0.221749901772], [0.00969874858856, 0.260340631008]] local_client.update_configuration({"footprint": base_footprint}) global_client.update_configuration({"footprint": base_footprint}) robot.head.cancel_goal() robot.publish_rack() return 'done' @cb_interface(outcomes=['done']) def _retract(_): robot.head.look_down() goal_pose = PoseStamped() goal_pose.header.stamp = rospy.Time.now() goal_pose.header.frame_id = rack_id goal_pose.pose.orientation = Quaternion( *quaternion_from_euler(0, 0, -math.pi / 2)) goal_pose.pose.position.x = 0.55 goal_pose.pose.position.y = -0.1 ControlToPose( robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.02, 0.1)).execute({}) return 'done' with self: self.add('PRE_GRAB', CBState(_pre_grab), transitions={'done': 'ALIGN_GRAB'}) self.add('ALIGN_GRAB', CBState(_align), transitions={'done': 'GRAB'}) self.add('GRAB', CBState(_grab), transitions={'done': 'RETRACT'}) self.add('RETRACT', CBState(_retract), transitions={'done': 'done'})
class MyStateMachine(Robot, StateMachine): def __init__(self, sim=False): super(MyStateMachine, self).__init__(sim) StateMachine.__init__(self) dsrv = DynamicReconfigureServer(RobotConfig, self.callback) self.dclient = DynamicReconfigureClient("core", timeout=30, config_callback=None) self.mir = MIR(HOST) def __del__(self): self.dclient.update_configuration({"start": False}) self.dclient.update_configuration({"go_home": False}) def callback(self, config, level): self.start = config['start'] self.go_home = config['go_home'] self.arm_task = config['arm_task'] return config idle = State('Idle', initial=True) home = State('Home') move = State('Move') arm = State('Arm') ## rmove means Relative move rmove = State('Rmove') toIdle = move.to(idle) | home.to(idle) | arm.to(idle) | rmove.to(idle) toHome = idle.to(home) | move.to(home) | home.to.itself() | arm.to(home) toMove = idle.to(move) | home.to(move) | move.to.itself() | arm.to( move) | rmove.to(move) toArm = move.to(arm) | idle.to(arm) toRove = idle.to(rmove) | move.to(rmove) | rmove.to.itself() def on_toIdle(self): print("To IDLE") ## TODO: STOP the robot arm # self.call_arm("stop") print("[IDLE] cancel arm task") self.cancel_arm_task() print("[IDLE] clear MiR mission queue") self.mir.clear_mission_queue() print("[IDLE] change MiR to 'Pause'") self.mir.set_status("Pause") print("[IDLE] Delete mission 'TKU_TMP'") self.mir.delete_mission("TKU_TMP") print("[IDLE] reset configures") self.dclient.update_configuration({"start": False}) self.dclient.update_configuration({"go_home": False}) def on_toHome(self): self.toMove("TKU_ToHOME") def on_toMove(self, mission): self.mir.set_status("Ready") print("to Move with mission {}".format(mission)) guid = self.mir.get_mission_guid(mission) if guid is None: warnings.warn("[WARRING] No this position name!!!!!") else: self.mir.add_mission_to_queue(guid.encode('utf-8')) # elif action.upper() == "RELATIVE MOVE": # self.mir.relative_move(dx=0.05) def on_toArm(self, mission=None): if mission is None: mission = self.arm_task print("Call Arm with mission '{}'".format(mission)) self.call_arm(mission)
bridge = CvBridge() image = None def callback(image_msg): try: cv_image = bridge.imgmsg_to_cv2(image_msg, 'bgr8') except CvBridgeError as e: rospy.logerr(e) res = detect_face(image_msg) print(res.key_points) for kp in res.key_points: cv2.circle(cv_image, (int(kp.x), int(kp.y)), 3, (0, 0, 255), 1) global image image = cv_image if __name__ == '__main__': rospy.init_node('visualize_openpose') detect_face = rospy.ServiceProxy('detect_face', DetectKeyPoints) detect_face.wait_for_service() reconf = ReconfClient('openpose') reconf.update_configuration({'key_point_threshold': 0.05}) image_sub = rospy.Subscriber('image', Image, callback) while not rospy.is_shutdown(): if image is not None: cv2.imshow('OpenPose visualization', image) cv2.waitKey(1)
class DelegationManagerTest(unittest.TestCase): """ Unit tests for the DelegationManager """ def __init__(self, *args, **kwargs): super(DelegationManagerTest, self).__init__(*args, **kwargs) rospy.init_node(name="DelegationManagerTest", log_level=rospy.DEBUG) def setUp(self): self.mocked_DM_name = "MockedDM" self.mocked_manager_name = "MockedManager" self.uut_name = "UUT" self.uut_mocked_manager_name = "UUTManager" self.mocked_client = MockedClient() self.mocked_client_id = self.mocked_client.id self.mocked_DM = MockedDelegationCommunicator( name=self.mocked_DM_name, manager_name=self.mocked_manager_name) self.mocked_cost_eval = MockedCostEvaluator(cost=0, possibility=True) self.standard_depth = 2 self.standard_members = ["Member1", "Member2"] self.uut = self.new_uut() rospy.sleep(0.3) self.dynrecClient = DynRecClient("UUT/delegation_module_parameters") self.dynrecClient.update_configuration({"max_tasks": 1}) def tearDown(self): self.mocked_DM.__del__() self.uut.__del__() def new_uut(self): """ Deletes old Unit under Test, UUT, and creates a new one. Also resets messages and config :return: uut :rtype: DelegationManager """ if hasattr(self, 'uut'): self.uut.__del__() self.uut = None rospy.sleep(0.5) uut = DelegationManager(name=self.uut_name) self.mocked_DM.reset_messages() self.uut = uut self.dynrecClient = None while not self.dynrecClient: try: self.dynrecClient = DynRecClient( "/UUT/delegation_module_parameters", timeout=1) except rospy.ROSInterruptException: pass rospy.sleep(0.3) rospy.logwarn( "NO DynRecClient created yet, waiting for dynamic_reconfigure services." ) return uut def test_setter_getter(self): """ Tests the properties """ rospy.loginfo("") uut = self.new_uut() self.assertEqual(uut.name, self.uut_name) curr_id = uut.get_new_auction_id() self.assertEqual(uut.get_new_auction_id(), curr_id + 1) # TODO this test is running fine if executed manually from pycharm but makes problems from the commandline @unittest.SkipTest def test_multiple_tasks(self): """ Tests adding tasks """ rospy.loginfo("test_multiple_tasks") id1, id2 = 1, 2 n1, n2 = "name1", "name2" g1, g2 = "goal1", "goal2" task1 = Task(auction_id=id1, auctioneer_name=n1, goal_name=g1, depth=self.standard_depth) task2 = Task(auction_id=id2, auctioneer_name=n2, goal_name=g2, depth=self.standard_depth) # Max Tasks = -1 (infinite) self.dynrecClient.update_configuration({"max_tasks": -1}) rospy.sleep(0.3) uut = self.new_uut() rospy.sleep(1) self.assertTrue(uut.check_possible_tasks) uut.add_task(new_task=task2) uut.add_task(new_task=task1) self.assertTrue(uut.check_possible_tasks) # Max Tasks = 0 self.dynrecClient.update_configuration({"max_tasks": 0}) rospy.sleep(0.3) uut = self.new_uut() self.assertFalse(uut.check_possible_tasks) self.assertRaises(DelegationError, uut.add_task, task1) self.assertRaises(LookupError, uut.get_task_by_goal_name, g1) # Max tasks = 1 self.dynrecClient.update_configuration({"max_tasks": 1}) rospy.sleep(0.3) uut = self.new_uut() self.assertTrue(uut.check_possible_tasks) uut.add_task(new_task=task1) self.assertFalse(uut.check_possible_tasks) self.assertRaises(DelegationError, uut.add_task, task2) self.assertEqual(uut.get_task_by_goal_name(goal_name=g1), task1) self.assertRaises(LookupError, uut.get_task_by_goal_name, g2) # Max tasks > 1 self.dynrecClient.update_configuration({"max_tasks": 2}) rospy.sleep(0.3) uut = self.new_uut() self.assertTrue(uut.check_possible_tasks) uut.add_task(new_task=task1) self.assertTrue(uut.check_possible_tasks) uut.add_task(new_task=task2) self.assertEqual(uut.get_task_by_goal_name(goal_name=g1), task1) self.assertEqual(uut.get_task_by_goal_name(goal_name=g2), task2) self.assertFalse(uut.check_possible_tasks) def test_cfp_callback(self): """ Tests CFP callback """ rospy.loginfo("test_cfp_callback") uut = self.new_uut() auction_id = 1 goal_rep = "test_goal" depth = 2 # No Manager present self.mocked_DM.send_cfp(auction_id=auction_id, goal_representation=goal_rep, depth=depth, delegation_members=self.standard_members) rospy.sleep(1) self.assertFalse(self.mocked_DM.got_pro) # "Manager" present, cost computable uut.set_cost_function_evaluator( self.mocked_cost_eval, client_id=self.mocked_client_id, agent_name=self.uut_mocked_manager_name) self.mocked_DM.send_cfp(auction_id=auction_id, goal_representation=goal_rep, depth=depth, delegation_members=self.standard_members) rospy.sleep(1) self.assertTrue(self.mocked_DM.got_pro) res = self.mocked_DM.Pro_last self.assertEqual(res.name, self.uut_name) self.assertEqual(res.auction_id, auction_id) self.assertEqual(res.value, 0) # the evaluator will always compute a 0 self.mocked_DM.reset_messages() # Deleted Manager uut.remove_cost_function_evaluator() self.mocked_DM.send_cfp(auction_id=auction_id, goal_representation=goal_rep, depth=depth, delegation_members=self.standard_members) rospy.sleep(1) self.assertFalse(self.mocked_DM.got_pro) def test_delegate(self): """ Tests delegate """ rospy.loginfo("test_delegate") uut = self.new_uut() goal_name = "test goal" test_goal = MockedGoalWrapper(name=goal_name) steps = 2 rospy.sleep(1) # ros msgs directly after startup are lost sometimes auction_id = uut.delegate(goal_wrapper=test_goal, auction_steps=steps, client_id=self.mocked_client_id) rospy.sleep(1) self.assertTrue(self.mocked_DM.got_cfp) cfp = self.mocked_DM.CFP_last self.assertEqual(cfp.goal_representation, goal_name) self.assertEqual(cfp.name, self.uut_name) self.assertEqual(cfp.auction_id, auction_id) delegation = uut.get_delegation(auction_id=auction_id) self.assertIsInstance(delegation, Delegation) self.assertTrue(delegation.state.is_waiting_for_proposals()) self.assertEqual(delegation.auction_id, auction_id) self.assertEqual(delegation.client_id, self.mocked_client_id) def test_propose_callback(self): """ Tests PROPOSE callback """ rospy.loginfo("test_propose_callback") uut = self.new_uut() goal_name = "test_goal" test_goal = MockedGoalWrapper(name=goal_name) steps = 2 proposed_value = 2 rospy.sleep(1) # ros msgs directly after startup are lost sometimes auction_id = uut.delegate(goal_wrapper=test_goal, auction_steps=steps, client_id=self.mocked_client_id) rospy.sleep(1) cfp = self.mocked_DM.CFP_last self.mocked_DM.send_propose(value=proposed_value, target_name=cfp.name, auction_id=cfp.auction_id) rospy.sleep(1) delegation = uut.get_delegation(auction_id=auction_id) proposal = delegation.get_best_proposal() self.assertEqual(proposal.name, self.mocked_DM_name) self.assertEqual(proposal.value, proposed_value) def test_ending_auctions(self): """ Tests the Ending of auctions """ rospy.loginfo("test_ending_auctions") uut = self.new_uut() goal_name = "test_goal" test_goal = MockedGoalWrapper(name=goal_name) test_depth = 3 test_task = Task(auction_id=100, goal_name=goal_name, depth=test_depth, auctioneer_name="test") uut.set_cost_function_evaluator( client_id=1, cost_function_evaluator=self.mocked_cost_eval, agent_name=self.mocked_manager_name) uut.add_task(test_task) steps = 3 proposed_value = 2 # No proposals rospy.sleep(1) # ros msgs directly after startup are lost sometimes auction_id = uut.delegate(goal_wrapper=test_goal, auction_steps=steps, client_id=self.mocked_client_id) rospy.sleep(1) self.mocked_DM.reset_messages() for i in range(steps - 1): uut.do_step(delegation_ids=[auction_id]) rospy.sleep(1) self.assertFalse(self.mocked_DM.got_cfp) uut.do_step(delegation_ids=[auction_id]) rospy.sleep(1) self.assertTrue(self.mocked_DM.got_cfp) self.assertEqual(self.mocked_DM.CFP_last.auction_id, auction_id) uut.terminate(auction_id=auction_id) # a proposal, but not true anymore, not bidding anymore test_goal = MockedGoalWrapper(name=goal_name) auction_id = uut.delegate(goal_wrapper=test_goal, auction_steps=steps, client_id=self.mocked_client_id) rospy.sleep(1) self.mocked_DM.send_propose(value=proposed_value, target_name=self.uut_name, auction_id=auction_id) self.mocked_DM.reset_messages() self.mocked_DM.set_precom_response(acceptance=False, still_bidding=False, cost=proposed_value) for i in range(steps): uut.do_step(delegation_ids=[auction_id]) rospy.sleep(1) self.assertTrue(self.mocked_DM.got_pre) self.assertTrue(self.mocked_DM.got_cfp) uut.terminate(auction_id=auction_id) # a proposal, but not true anymore, still bidding test_goal = MockedGoalWrapper(name=goal_name) auction_id = uut.delegate(goal_wrapper=test_goal, auction_steps=steps, client_id=self.mocked_client_id) rospy.sleep(1) self.mocked_DM.send_propose(value=proposed_value, target_name=self.uut_name, auction_id=auction_id) self.mocked_DM.reset_messages() proposed_value += 1 self.mocked_DM.set_precom_response(acceptance=False, still_bidding=True, cost=proposed_value) for i in range(steps): uut.do_step(delegation_ids=[auction_id]) rospy.sleep(1) self.assertTrue(self.mocked_DM.got_pre) self.assertTrue(self.mocked_DM.got_cfp) uut.terminate(auction_id=auction_id) # a proposal, but no answer to precommit test_goal = MockedGoalWrapper(name=goal_name) auction_id = uut.delegate(goal_wrapper=test_goal, auction_steps=steps, client_id=self.mocked_client_id) rospy.sleep(1) self.mocked_DM.send_propose(value=proposed_value, target_name=self.uut_name, auction_id=auction_id) self.mocked_DM.reset_messages() self.mocked_DM.stop_communication() for i in range(steps): uut.do_step(delegation_ids=[auction_id]) rospy.sleep(1) self.assertTrue(self.mocked_DM.got_cfp) uut.terminate(auction_id=auction_id) self.mocked_DM.start_communication() # proposal and accepted precom test_goal = MockedGoalWrapper(name=goal_name) auction_id = uut.delegate(goal_wrapper=test_goal, auction_steps=steps, client_id=self.mocked_client_id) rospy.sleep(1) self.mocked_DM.send_propose(value=proposed_value, target_name=self.uut_name, auction_id=auction_id) self.mocked_DM.reset_messages() self.mocked_DM.set_precom_response(acceptance=True, still_bidding=True, cost=proposed_value) for i in range(steps): uut.do_step(delegation_ids=[auction_id]) rospy.sleep(1) self.assertTrue(self.mocked_DM.got_pre) self.assertEqual(self.mocked_DM.Pre_last.depth, test_depth + 1) self.assertTrue(test_goal.goal_is_created()) self.assertEqual(test_goal.get_manager(), self.mocked_manager_name) uut.terminate(auction_id=auction_id) # own cost wins test_goal = MockedGoalWrapper(name=goal_name) own_cost = 2 auction_id = uut.delegate(goal_wrapper=test_goal, auction_steps=steps, client_id=self.mocked_client_id, own_cost=own_cost) for i in range(steps): uut.do_step(delegation_ids=[auction_id]) rospy.sleep(1) self.assertFalse(test_goal.goal_is_created()) self.assertTrue(self.mocked_client.started_working) # TODO this test is running fine if executed manually from pycharm but makes problems from the commandline @unittest.SkipTest def test_terminate(self): """ Tests terminate """ rospy.loginfo("test_terminate") uut = self.new_uut() goal_name = "test_goal" test_goal = MockedGoalWrapper(name=goal_name) steps = 3 proposed_value = 2 # proposal and accepted precom auction_id = uut.delegate(goal_wrapper=test_goal, auction_steps=steps, client_id=self.mocked_client_id) rospy.sleep(1) self.mocked_DM.send_propose(value=proposed_value, target_name=self.uut_name, auction_id=auction_id) self.mocked_DM.reset_messages() self.mocked_DM.set_precom_response(acceptance=True, still_bidding=True, cost=proposed_value) for i in range(steps): uut.do_step(delegation_ids=[auction_id]) rospy.sleep(1) self.assertTrue(self.mocked_DM.got_pre) self.assertTrue(test_goal.goal_is_created()) self.assertEqual(test_goal.get_manager(), self.mocked_manager_name) uut.terminate(auction_id=auction_id) delegation = uut.get_delegation(auction_id=auction_id) self.assertFalse(test_goal.goal_is_created()) self.assertTrue(delegation.state.is_finished()) # TODO this test is running fine if executed manually from pycharm but makes problems from the commandline @unittest.SkipTest def test_precom_callback(self): """ Tests PRECOM callback """ rospy.loginfo("test_precom_callback") auction_id = 1 goal_name = "test_goal" old_proposal = 3 # cost not computable uut = self.new_uut() response = self.mocked_DM.send_precom( target_name=self.uut_name, auction_id=auction_id, proposal_value=old_proposal, goal_name=goal_name, goal_representation=goal_name, depth=self.standard_depth, delegation_members=self.standard_members) self.assertFalse(response.acceptance) self.assertFalse(response.still_biding) # max tasks = 0 self.dynrecClient.update_configuration({"max_tasks": 0}) rospy.sleep(0.3) uut = self.new_uut() uut.set_cost_function_evaluator( cost_function_evaluator=self.mocked_cost_eval, agent_name=self.uut_mocked_manager_name, client_id=self.mocked_client_id) response = self.mocked_DM.send_precom( target_name=self.uut_name, auction_id=auction_id, proposal_value=old_proposal, goal_name=goal_name, goal_representation=goal_name, depth=self.standard_depth, delegation_members=self.standard_members) self.assertFalse(response.acceptance) self.assertFalse(response.still_biding) # not possible anymore self.dynrecClient.update_configuration({"max_tasks": 1}) rospy.sleep(0.3) uut = self.new_uut() uut.set_cost_function_evaluator( cost_function_evaluator=MockedCostEvaluator(cost=0, possibility=False), agent_name=self.uut_mocked_manager_name, client_id=self.mocked_client_id) response = self.mocked_DM.send_precom( target_name=self.uut_name, auction_id=auction_id, proposal_value=old_proposal, goal_name=goal_name, goal_representation=goal_name, depth=self.standard_depth, delegation_members=self.standard_members) self.assertFalse(response.acceptance) self.assertFalse(response.still_biding) # cost is worse now new_cost = old_proposal + 1 self.dynrecClient.update_configuration({"max_tasks": 1}) rospy.sleep(0.3) uut = self.new_uut() uut.set_cost_function_evaluator( cost_function_evaluator=MockedCostEvaluator(cost=new_cost, possibility=True), agent_name=self.uut_mocked_manager_name, client_id=self.mocked_client_id) response = self.mocked_DM.send_precom( target_name=self.uut_name, auction_id=auction_id, proposal_value=old_proposal, goal_name=goal_name, goal_representation=goal_name, depth=self.standard_depth, delegation_members=self.standard_members) self.assertFalse(response.acceptance) self.assertTrue(response.still_biding) self.assertEqual(response.new_proposal, new_cost) # cost is exactly the same self.dynrecClient.update_configuration({"max_tasks": -1}) rospy.sleep(0.3) new_cost = old_proposal uut = self.new_uut() uut.set_cost_function_evaluator( cost_function_evaluator=MockedCostEvaluator(cost=new_cost, possibility=True), agent_name=self.uut_mocked_manager_name, client_id=self.mocked_client_id) response = self.mocked_DM.send_precom( target_name=self.uut_name, auction_id=auction_id, proposal_value=old_proposal, goal_name=goal_name, goal_representation=goal_name, depth=self.standard_depth, delegation_members=self.standard_members) self.assertTrue(response.acceptance) self.assertEqual(response.manager_name, self.uut_mocked_manager_name) def test_cost_function_adding_removing(self): """ Tests CostEvaluator adding/removing """ rospy.loginfo("test_cost_function_adding_removing") uut = self.new_uut() uut.set_cost_function_evaluator( cost_function_evaluator=self.mocked_cost_eval, agent_name=self.uut_mocked_manager_name, client_id=self.mocked_client_id) self.assertTrue(uut.cost_computable) uut.remove_cost_function_evaluator() self.assertFalse(uut.cost_computable) def test_fail_task(self): """ Tests FAIL callback """ rospy.loginfo("test_fail_task") uut = self.new_uut() auction_id = 1 goal_name = "test goal" task = Task(auction_id=auction_id, auctioneer_name=self.mocked_DM_name, goal_name=goal_name, depth=self.standard_depth) uut.add_task(new_task=task) uut.fail_task(goal_name=goal_name) rospy.sleep(1) self.assertTrue(self.mocked_DM.got_fail) request = self.mocked_DM.Fail_last self.assertEqual(self.uut_name, request.name) self.assertEqual(request.auction_id, auction_id) def test_end_task(self): """ Tests end task """ rospy.loginfo("test_end_task") uut = self.new_uut() auction_id = 1 goal_name = "test goal" task = Task(auction_id=auction_id, auctioneer_name=self.mocked_DM_name, goal_name=goal_name, depth=self.standard_depth) uut.add_task(new_task=task) uut.end_task(goal_name=goal_name) self.assertRaises(LookupError, uut.get_task_by_goal_name, goal_name)
class Camera: def __init__(self, node_name, proj, level, **paramnames): self.paramnames = paramnames self.name = node_name self.level = level self.proj = proj self.reconfigure_client = None self. async = AsynchronousUpdater(self.async_apply_update, "Camera " + node_name) self.trig_rising = True def param(self, config, name): return config[self.paramnames[name]] def setparam(self, config, name, value): config[self.paramnames[name]] = value # Uses the new configuration to compute new camera state. def process_update(self, config, level): # Produces: # MEMBERS # period, ext_trig, imager_period, end_offset # # PARAMETERS # "param_rate" self.trigger_name = "not_set" # Will be set by the CameraTriggerController # Took level checking out as it was causing problems with the projector # needed flag. #if self.level & level == 0: # return # This camera's configuration is unchanged. self.period = 1.0 / self.param(config, param_rate) projector_rate = config[param_proj_rate] self.reset_cameras = config["camera_reset"] self.ext_trig = True self.register_set = WGEConfig.WGE100Camera_PrimaryRegisterSet projector_limits_exposure = True trig_mode = self.param(config, param_trig_mode) # Internal triggering. if trig_mode == Config.CameraSynchronizer_InternalTrigger: self.ext_trig = False self.imager_period = self.period self.end_offset = -1 projector_limits_exposure = False else: if self.proj.mode == Config.CameraSynchronizer_ProjectorOff: trig_mode = Config.CameraSynchronizer_IgnoreProjector if trig_mode == Config.CameraSynchronizer_AlternateProjector or trig_mode == Config.CameraSynchronizer_WithProjector: self.proj.needed = True #print self.name, "setting needed to true." #else: #print self.name, "not setting needed to true." # Set the period if trig_mode == Config.CameraSynchronizer_AlternateProjector: n = round(self.period / self.proj.repeat_period - 0.5) n = max(n, 0) self.period = (n + 0.5) * self.proj.repeat_period #print "Case 1", n elif trig_mode == Config.CameraSynchronizer_IgnoreProjector: self.period = roundToEthercat(self.period) else: n = round(2 * self.period / self.proj.repeat_period - 1) n = max(n, 0) self.period = (n + 1) * self.proj.repeat_period / 2.0 #print "Case 2", n, self.period # Set the end_offset if trig_mode == Config.CameraSynchronizer_IgnoreProjector: self.end_offset = 0 projector_limits_exposure = False if trig_mode == Config.CameraSynchronizer_AlternateProjector: self.end_offset = self.proj.alt_end_offset self.register_set = WGEConfig.WGE100Camera_Auto elif trig_mode == Config.CameraSynchronizer_WithProjector: self.end_offset = self.proj.proj_end_offset self.register_set = WGEConfig.WGE100Camera_AlternateRegisterSet else: self.end_offset = self.proj.noproj_end_offset # Pick the imager period if trig_mode == Config.CameraSynchronizer_IgnoreProjector: self.imager_period = self.period - ETHERCAT_INTERVAL else: self.imager_period = self.proj.repeat_period / 2 - ETHERCAT_INTERVAL #print "Camera period", self.name, self.period, self.imager_period, self.proj.repeat_period if projector_limits_exposure: self.max_exposure = self.proj.noproj_max_exposure #print "Exposurei projector", self.max_exposure else: self.max_exposure = self.imager_period * 0.95 #print "Exposurei imager", self.max_exposure self.setparam(config, param_rate, 1 / self.period) self.setparam(config, param_trig_mode, trig_mode) def async_apply_update(self, reconfig_request): try: #print "**** Start", self.name if self.reconfigure_client == None: #print "**** Making client", self.name self.reconfigure_client = DynamicReconfigureClient(self.name) #print "**** Made client", self.name self.reconfigure_client.update_configuration(reconfig_request) #print "**** Reconfigured client", self.name #print "Done updating camera ", self.name except KeyboardInterrupt: # Handle CTRL+C print "Aborted camera update on", self.name def apply_update(self): reconfig_request = { "ext_trig": self.ext_trig, "trig_rate": 1.0 / self.period, "imager_rate": 1.0 / self.imager_period, "trig_timestamp_topic": self.trigger_name, "rising_edge_trig": self.trig_rising, "register_set": self.register_set, "auto_exposure_alternate": False, "exposure_alternate": self.proj.proj_exposure_duration, "max_exposure": self.max_exposure, } #print self.name, reconfig_request self. async .update(reconfig_request)
class Explore(EventState): ''' Starts the Exploration Task via /move_base ># speed Speed of the robot <= succeeded Exploration Task was successful <= failed Exploration Task failed ''' def __init__(self): super(Explore, self).__init__(outcomes = ['succeeded', 'failed'], input_keys =['speed']) self._action_topic = '/move_base' self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction}) self._dynrec = Client("/vehicle_controller", timeout = 10) self._defaultspeed = 0.1 self._succeeded = False self._failed = False def execute(self, userdata): if self._move_client.has_result(self._action_topic): result = self._move_client.get_result(self._action_topic) self._dynrec.update_configuration({'speed':self._defaultspeed}) if result.result == 1: self._reached = True Logger.loginfo('Exploration succeeded') return 'succeeded' else: self._failed = True Logger.logwarn('Exploration failed!') return 'failed' def on_enter(self, userdata): speedValue = self._dynrec.get_configuration(timeout = 0.5) if speedValue is not None: self._defaultspeed = speedValue['speed'] self._dynrec.update_configuration({'speed':userdata.speed}) self._succeeded = False self._failed = False action_goal = MoveBaseGoal() action_goal.exploration = True action_goal.speed = userdata.speed if action_goal.target_pose.header.frame_id == "": action_goal.target_pose.header.frame_id = "world" try: if self._move_client.is_active(self._action_topic): self._move_client.cancel(self._action_topic) self._move_client.send_goal(self._action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to start Exploration' % { 'err': str(e), 'x': userdata.waypoint.pose.position.x, 'y': userdata.waypoint.pose.position.y }) self._failed = True def on_exit(self, userdata): self._move_client.cancel(self._action_topic) def on_start(self): pass def on_stop(self): pass