def check_users_exist(self): gone = [] for uid, user in self.users_.items(): if user.exists_ == False: gone.append(uid) for g in gone: if rospy.has_param("wallframe/user_data/" + str(g)): rospy.delete_param("wallframe/user_data/" + str(g)) rospy.logwarn("User [wallframe UID: " + str(g) + "] lost, removing from list") self.toast_pub_.publish(String("User " + str(g) + " Lost")) msg = user_event_msg() msg.user_id = g msg.event_id = 'workspace_event' msg.message = 'user_left' self.user_event_pub_.publish(msg) del self.users_[g] if len(self.users_) == 0: if self.no_users_ == False: # Send all users left message msg = user_event_msg() msg.user_id = g msg.event_id = 'workspace_event' msg.message = 'all_users_left' self.user_event_pub_.publish(msg) self.no_users_ = True
def __init__(self): try: self._map = rospy.get_param("~map") except KeyError as e: rospy.logfatal("map param not set!: %s " % e) raise MapSetupException("map param not set") # Clean/delete params: try: rospy.delete_param("mmap") except: pass # Publish map_in_use (latched): self._map_in_use_pub = rospy.Publisher("map_in_use", String, queue_size=1, latch=True) self._map_in_use_pub.publish("submap_0") # Set params: rospy.set_param("map_package_in_use" , "deprecated") rospy.set_param("nice_map_in_use" , os.path.join(self._map, "map.bmp")) rospy.set_param("map_transformation_in_use", os.path.join(self._map, "transformation.xml")) # Load paramlist = rosparam.load_file(os.path.join(self._map, "mmap.yaml"), "mmap") for param, ns in paramlist: try: rosparam.upload_params(ns, param) except: pass # ignore empty params
def __init__(self): rospy.logwarn('clearing parameters') rospy.delete_param('~rplidar_scan_topic') rospy.delete_param('~rplidar_port_name') rospy.delete_param('~rplidar_frame') rospy.delete_param('~rplidar_range_min') rospy.delete_param('~rplidar_range_max')
def cb_rem_master_model(f): print "##### start cb_remove_master_model " +datetime.datetime.today().strftime("%Y_%m_%d_%H%M%S") # error_code,messageの初期化 publish_retCode_and_message("","") ret = remove_master_model() if(ret == 501): publish_retCode_and_message(str(ret),str("REMOVE MASTER:recipe_id not registerd")) elif(ret == 502): publish_retCode_and_message(str(ret),str("REMOVE MASTER:remove master failed")) # 削除したマスターが現在選択中の場合には、/mt/masterパラメータを削除する # パラメータ取得 # レシピID remove_recipe_id = rospy.get_param('/mt/master/recipe_id',None) if(remove_recipe_id == None): # recipe_idが無い場合はそのまま終了 publish_retCode_and_message(str(0),str("REMOVE MASTER:Success")) return else: if(len(recipe_id) == 0): # recipe_idが無い場合はそのまま終了 publish_retCode_and_message(str(0),str("REMOVE MASTER:Success")) return # /mt/masterパラメータを削除する if rospy.has_param('/mt/master'): rospy.delete_param('/mt/master') publish_retCode_and_message(str(0),str("REMOVE MASTER:Success")) print "##### end cb_remove_master_model " +datetime.datetime.today().strftime("%Y_%m_%d_%H%M%S") return
def param_demo(): rospy.init_node("param_demo") rate = rospy.Rate(1) while(not rospy.is_shutdown()): #get param parameter1 = rospy.get_param("/param1") parameter2 = rospy.get_param("/param2", default=222) rospy.loginfo('Get param1 = %d', parameter1) rospy.loginfo('Get param2 = %d', parameter2) #delete param rospy.delete_param('/param2') #set param rospy.set_param('/param2',2) #check param ifparam3 = rospy.has_param('/param3') if(ifparam3): rospy.loginfo('/param3 exists') else: rospy.loginfo('/param3 does not exist') #get all param names params = rospy.get_param_names() rospy.loginfo('param list: %s', params) rate.sleep()
def main(): try: # For testing purposes try: rospy.delete_param("is_initialized") except: pass # Unpause the physics rospy.loginfo("Unpausing Gazebo...") rospy.wait_for_service('/gazebo/unpause_physics') unpause_gazebo = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) resp = unpause_gazebo() rospy.loginfo("Unpaused Gazebo.") example = ExampleInitializeGazeboRobot() success = example.is_init_success if success: success &= example.home_the_robot() except Exception as e: print (e) success = False # For testing purposes rospy.set_param("is_initialized", success) if not success: rospy.logerr("The Gazebo initialization encountered an error.") else: rospy.loginfo("The Gazebo initialization executed without fail.")
def __init__(self): try: self._map = rospy.get_param("~map") except KeyError as e: rospy.logfatal("map param not set!: %s " % e) raise MapSetupException("map param not set") # Clean/delete params: try: rospy.delete_param("mmap") except: pass # Publish map_in_use (latched): self._map_in_use_pub = rospy.Publisher("map_in_use", String, queue_size=1, latch=True) self._map_in_use_pub.publish("submap_0") # Set params: rospy.set_param("map_package_in_use", "deprecated") rospy.set_param("nice_map_in_use", os.path.join(self._map, "map.bmp")) rospy.set_param("map_transformation_in_use", os.path.join(self._map, "transformation.xml")) # Load paramlist = rosparam.load_file(os.path.join(self._map, "mmap.yaml"), "mmap") for param, ns in paramlist: try: rosparam.upload_params(ns, param) except: pass # ignore empty params
def clear_params(node_name, params): for k in params: try: rospy.delete_param("{}/{}".format(node_name, k)) except KeyError: print("value {} not set".format(k)) return True
def main(): rospy.init_node('encoders_acq_node', anonymous=True) """Module initialization""" for joint_name in joints: if rospy.has_param("traction_hw/joints/{}".format(joint_name)): joint_hw = rospy.get_param("traction_hw/joints/{}".format(joint_name)) exec("{} = EncoderAcq('{}')".format(joint_name, joint_name)) rate_param = rospy.get_param("traction_hw/sampling_frequency", 100) rate = rospy.Rate(rate_param) rospy.loginfo("[Encoders] Reading sensor data...") while not rospy.is_shutdown(): if 'right_wheel' in locals(): right_wheel.get_sensor_data() if 'left_wheel' in locals(): left_wheel.get_sensor_data() rate.sleep() for joint_name in joints: if 'right_wheel' in locals(): right_wheel.spi_bus.close() if 'left_wheel' in locals(): left_wheel.spi_bus.close() """Clean up ROS parameter server""" try: rospy.delete_param("exo_traction") rospy.delete_param("spi_comm") except KeyError: pass
def shutdown(self): """ Delete any remaining parameters so they don't affect next launch """ for param in [self._device_name_param, self._lm_param, self._dic_param]: if rospy.has_param(param): rospy.delete_param(param) """ Shutdown the GTK thread. """
def remote_destroy(self): """ Method should be called to delete the Parameter from the parameter server. """ if self._registered: try: rospy.delete_param(self._name) except rospy.ROSException: pass self._registered = False if self._owner: self._owner.unregisterParameter(self) self._owner = None if self._status: def eb(failure): if not failure.check(PBConnectionLost): log.err(failure) try: self._status.callRemote('died').addErrback(eb) except (DeadReferenceError, PBConnectionLost): pass self._status = None
def check_users_exist(self): gone = [] for uid, user in self.users_.items(): if user.exists_ == False: gone.append(uid) for g in gone: if rospy.has_param("wallframe/user_data/"+str(g)): rospy.delete_param("wallframe/user_data/"+str(g)) rospy.logwarn("User [wallframe UID: " + str(g) + "] lost, removing from list") self.toast_pub_.publish(String("User "+str(g)+" Lost")) msg = user_event_msg() msg.user_id = g msg.event_id = 'workspace_event' msg.message = 'user_left' self.user_event_pub_.publish(msg) del self.users_[g] if len(self.users_) == 0: if self.no_users_ == False: # Send all users left message msg = user_event_msg() msg.user_id = g msg.event_id = 'workspace_event' msg.message = 'all_users_left' self.user_event_pub_.publish(msg) self.no_users_ = True
def param_demo(): rospy.init_node("param_demo") rate = rospy.Rate(1) while (not rospy.is_shutdown()): #get param parameter1 = rospy.get_param("/param1") parameter2 = rospy.get_param("/param2", default=222) rospy.loginfo('Get param1 = %d', parameter1) rospy.loginfo('Get param2 = %d', parameter2) #delete param rospy.delete_param('/param2') #set param rospy.set_param('/param2', 2) #check param ifparam3 = rospy.has_param('/param3') if (ifparam3): rospy.loginfo('/param3 exists') else: rospy.loginfo('/param3 does not exist') #get all param names params = rospy.get_param_names() rospy.loginfo('param list: %s', params) rate.sleep()
def main(): rospy.init_node('elevation_acq_node', anonymous=True) """Module initialization""" if rospy.has_param("elevation_hw/joints/elevation"): joint_hw = rospy.get_param("elevation_hw/joints/elevation") can_id = rospy.get_param("elevation_hw/joints/elevation/can_id") has_hw = [] for hw_component in hw_names: has_hw.append(joint_hw[hw_component]) if True in has_hw: elevation = SensorAcq('elevation', has_hw) # CAN can_channel = rospy.get_param("can_comm/elevation_port", "can1") can_bus = CANbus(channel=can_channel) rate_param = rospy.get_param("elevation_hw/sampling_frequency", 100) rate = rospy.Rate(rate_param) rospy.loginfo("[Elevation Acquisition] Reading sensor data...") while not rospy.is_shutdown(): can_bus.send_command() msg = can_bus.receive_data() if msg is not None: elevation.get_sensor_data(msg) rate.sleep() """Clean up ROS parameter server""" try: rospy.delete_param("exo_hw") rospy.delete_param("can_comm") except KeyError: pass
def reset_demo(self): """Reset the environment for demo collection episodes. same functionality as "reset" method. """ # Desired goal is given here. # Should wait until reset finishes. if rospy.has_param('vel_calc'): rospy.delete_param('vel_calc') self.control_command[-1] = float(0) if not self.isReal: # for simulated env. des_goal = self._reset_gazebo() else: des_goal = self._reset_real() _joint_pos, _joint_vels, _joint_effos = self.get_joints_states() _color_obs = self.get_color_observation() _targ_obj_obs = self.get_target_obj_obs() _gripper_pos = self.get_gripper_position() _ee_pose = self.get_end_effector_pose() obs = dict() obs['full_state'] = [_joint_pos, _joint_vels, _joint_effos] # default observations if self.isGripper: obs['full_state'].append([_gripper_pos]) if self.isCartesian: obs['full_state'].append(_ee_pose) if self.isPOMDP: obs['color_obs'] = _color_obs rospy.set_param('vel_calc','true') return {'observation': obs,'desired_goal':des_goal, 'auxiliary':_targ_obj_obs}
def test_one_hand_no_mapping_no_robot_description_finder(self): if rospy.has_param("hand"): rospy.delete_param("hand") if rospy.has_param("robot_description"): rospy.delete_param("robot_description") rospy.set_param("hand/joint_prefix/1", "rh_") rospy.set_param("hand/mapping/1", "") hand_finder = HandFinder() # hand params self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.") self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping") self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 1, "It should be only one joint_prefix") self.assertEqual(hand_finder.get_hand_parameters().mapping['1'], "1", "It should be the serial id as mapping") self.assertEqual(hand_finder.get_hand_parameters().joint_prefix['1'], "rh_", "It should be only rh_ prefix") # hand joints self.assertIsNotNone(hand_finder.get_hand_joints(), "Joints extracted.") self.assertEqual(len(list(hand_finder.get_hand_joints().keys())), 1, "It should be only one mapping") print(hand_finder.get_hand_joints()) self.assertIn("1", hand_finder.get_hand_joints(), "Serial should be in the joints result") joints = hand_finder.get_hand_joints()['1'] # use serial self.assertEqual(len(joints), 24, "Joint number should be 24") self.assertIn("rh_FFJ3", hand_finder.get_hand_joints()["1"], "FFJ3 joint should be in the joints list")
def main(): try: # For testing purposes try: rospy.delete_param("is_initialized") except: pass # Unpause the physics # This will let MoveIt finish its initialization rospy.loginfo("Unpausing") rospy.wait_for_service('/gazebo/unpause_physics') unpause_gazebo = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) resp = unpause_gazebo() rospy.loginfo("Unpaused") example = ExampleInitializeGazeboRobot() rospy.loginfo("Created example") success = example.is_init_success rospy.loginfo("success = {}".format(success)) if success: success &= example.home_the_robot() except: success = False # For testing purposes rospy.set_param("is_initialized", success) if not success: rospy.logerr("The Gazebo initialization encountered an error.") else: rospy.loginfo("The Gazebo initialization executed without fail.")
def __init__(self): rospy.delete_param('~fussion_use_rplidar_topic') rospy.delete_param('~fussion_use_asus_topic') rospy.delete_param('~fussion_scan_topic') rospy.delete_param('~fussion_PublishFrequency') rospy.delete_param('~fussion_asus_max_range')
def spin(self): #-----sync----- now = rospy.get_rostime() s, ns = now.secs, now.nsecs target = self.sync_time ns = ns * 10**(-9) diff = target - (s + ns) print "diff", diff #sleep until sync time rospy.sleep(diff) self.pid.clear() self.last_time = rospy.get_time() self.latest_msg = [rospy.get_time()]*self.nr_cars rate = rospy.Rate(config.rate) first = True while not rospy.is_shutdown(): rate.sleep() self.update() if self.id == 0 and first: rospy.delete_param("sync_time") rospy.delete_param("random") first = False
def test_param_api(self): # test get_param_names param_names = rospy.get_param_names() for n in ['/param1', 'param1', '~param1', 'param_int', 'param_float']: self.assert_(rospy.resolve_name(n) in param_names) # test has_param self.assert_(rospy.has_param('/run_id')) self.assert_(rospy.has_param('/param1')) self.assert_(rospy.has_param('param1')) self.assert_(rospy.has_param('~param1')) # test search_param self.assertEquals(None, rospy.search_param('not_param1')) self.assertEquals(rospy.resolve_name('~param1'), rospy.search_param('param1')) self.assertEquals(rospy.resolve_name('parent_param'), rospy.search_param('parent_param')) self.assertEquals("/global_param", rospy.search_param('global_param')) # test get_param on params set in rostest file self.assertEquals('global_param1', rospy.get_param('/param1')) self.assertEquals('parent_param1', rospy.get_param('param1')) self.assertEquals('private_param1', rospy.get_param('~param1')) # - type tests self.assertEquals(1, rospy.get_param('param_int')) self.assertAlmostEquals(2., rospy.get_param('param_float')) self.assertEquals(True, rospy.get_param('param_bool')) self.assertEquals("hello world", rospy.get_param('param_str')) # test default behavior get_param try: rospy.get_param('not_param1') self.fail("should have raised KeyError") except KeyError: pass self.assertEquals('parent_param1', rospy.get_param('param1', 'foo')) self.assertEquals('private_param1', rospy.get_param('~param1', 'foo')) self.assertEquals('myval', rospy.get_param('not_param1', 'myval')) self.assertEquals('myval', rospy.get_param('~not_param1', 'myval')) self.assertEquals(None, rospy.get_param('not_param1', None)) self.assertEquals(None, rospy.get_param('~not_param1', None)) # test set/get roundtrips vals = [ '1', 1, 1., [1, 2, 3, 4], {'a': 1, 'b': 2}] for v in vals: self.failIf(rospy.has_param("cp_param")) try: rospy.get_param('cp_param1') self.fail("should have thrown KeyError") except KeyError: pass self.assertEquals(None, rospy.get_param('cp_param', None)) self.assertEquals("default", rospy.get_param('cp_param', "default")) rospy.set_param("cp_param", v) self.assert_(rospy.has_param("cp_param")) self.assertEquals(v, rospy.get_param("cp_param")) self.assertEquals(rospy.resolve_name('cp_param'), rospy.search_param('cp_param')) # erase the param and recheck state rospy.delete_param('cp_param') self.failIf(rospy.has_param("cp_param")) self.assertEquals(None, rospy.get_param('cp_param', None)) self.assertEquals("default", rospy.get_param('cp_param', "default")) self.assertEquals(None, rospy.search_param('cp_param'))
def main(self): # For testing purposes success = self.is_init_success try: rospy.delete_param( "/kortex_examples_test_results/actuator_configuration_python") except: pass if success: #------------------------------------------------------------- # Find actuators and set which one we want to configure success &= self.example_find_actuators_and_set_device_id() #------------------------------------------------------------- # Clear the faults on a specific actuator success &= self.example_clear_actuator_faults() #------------------------------------------------------------- # Get the control loop parameters on a specific actuator control_loop_parameters = None success &= self.example_get_control_loop_parameters( control_loop_parameters) # For testing purposes rospy.set_param( "/kortex_examples_test_results/actuator_configuration_python", success) if not success: rospy.logerr("The example encountered an error.")
def set_lane(self, next): # to next node if next: self.last_node += 1 else: self.last_node -= 1 if self.last_node >= len(self.guid_path) - 1: rospy.loginfo("It's the last lane.") self.last_node -= 1 return elif self.last_node < 0: rospy.loginfo("Back to first lane.") self.last_node = 0 # set last, next node (anchor) rospy.set_param("/guid_lane_last", self.all_anchor_ids[self.guid_path[self.last_node]]) rospy.set_param( "/guid_lane_next", self.all_anchor_ids[self.guid_path[self.last_node + 1]]) # set pozyx ranging tag id try: rospy.delete_param("/anchorballs") except KeyError: rospy.logerr("No such param /anchorballs") pozyx_id = {} pozyx_id[self.guid_path[self.last_node]] = self.all_anchor_ids[ self.guid_path[self.last_node]] pozyx_id[self.guid_path[self.last_node + 1]] = self.all_anchor_ids[ self.guid_path[self.last_node + 1]] rospy.set_param("/anchorballs", pozyx_id)
def _set_param_and(self): params = { 'countermeasure': { 'config': { 'storage_timeout': 10, 'reaction_autonomy_level': 50}, 'constraints': { 'john': { 'reactions': { 'two': { 'action': 'publish', 'node': 'node1', 'message': 'node1 has a problem', 'loglevel': 'info', 'autonomy_level': 13}, 'one': { 'action': 'stop', 'node': 'node1', 'autonomy_level': 100}}, 'min_reaction_interval': 5, 'constraint': { 'or': { 'n!node2': {'ram_usage_mean': 'high'}, 'n!node1': {'cpu_usage_mean': 'high'}}}}}}} if rospy.has_param("/arni"): rospy.delete_param("/arni") rospy.set_param("/arni", params)
def ros(): try: logging.loginfo(u'deleting tmp test folder') except Exception: pass logging.loginfo(u'init ros') rospy.init_node(u'tests') tf_init(60) launch = roslaunch.scriptapi.ROSLaunch() launch.start() rospy.set_param('/joint_trajectory_splitter/state_topics', [ '/whole_body_controller/base/state', '/whole_body_controller/body/state' ]) rospy.set_param('/joint_trajectory_splitter/client_topics', [ '/whole_body_controller/base/follow_joint_trajectory', '/whole_body_controller/body/follow_joint_trajectory' ]) node = roslaunch.core.Node('giskardpy', 'joint_trajectory_splitter.py', name='joint_trajectory_splitter') joint_trajectory_splitter = launch.launch(node) yield joint_trajectory_splitter.stop() rospy.delete_param('/joint_trajectory_splitter/state_topics') rospy.delete_param('/joint_trajectory_splitter/client_topics') logging.loginfo(u'shutdown ros') rospy.signal_shutdown(u'die') try: logging.loginfo(u'deleting tmp test folder') except Exception: pass
def test_no_hand_robot_description_exists_finder(self): if rospy.has_param("hand"): rospy.delete_param("hand") if rospy.has_param("robot_description"): rospy.delete_param("robot_description") rospy.set_param("robot_description", rospy.get_param("two_hands_description")) hand_finder = HandFinder() self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 0, "correct parameters without a hand") self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 0, "correct parameters without a hand") self.assertEqual(len(hand_finder.get_hand_joints()), 0, "correct joints without a hand") self.assertEqual(len(hand_finder.get_calibration_path()), 0, "correct calibration path without a hand") self.assertEqual( len(hand_finder.get_hand_control_tuning().friction_compensation), 0, "correct tuning without a hands") self.assertEqual( len(hand_finder.get_hand_control_tuning().host_control), 0, "correct tuning without a hands") self.assertEqual( len(hand_finder.get_hand_control_tuning().motor_control), 0, "correct tuning without a hands")
def datastore_callback(self, name, value): if value is not None: rospy.loginfo('got new value of %r from Datastore', name) rospy.set_param(name, value) else: rospy.loginfo('parameter %r deleted from Datastore', name) rospy.delete_param(name, value)
def shutdown(self): """ Delete any remaining parameters so they don't affect next launch """ for param in [self._fsg_param]: if rospy.has_param(param): rospy.delete_param(param) """ Shutdown the GTK thread. """ gtk.main_quit()
def __init__(self): rospy.loginfo('Cleaning Parameters...') rospy.delete_param('~PlanTopic') rospy.delete_param('~OdomTopic') rospy.delete_param('~PositionFree') rospy.delete_param('~ActionPlanTopic') rospy.delete_param('~GoalTopic')
def test_param_withhold_update_disappear(self): """ Test param witholding functionality for a param which doesnt exists anymore in the ros environment. Normal usecase. Sequence : (-> UPDATE ?) -> WITHHOLD -> UPDATE -> DISAPPEAR (-> UPDATE ?) :return: """ paramname = '/test/absentparam1' # every added param should be in the list of args self.assertTrue(paramname not in self.param_if_pool.params_args) # the backend should not have been created self.assertTrue(paramname not in self.param_if_pool.params.keys()) self.param_if_pool.expose_params([paramname]) # every added param should be in the list of args self.assertTrue(paramname in self.param_if_pool.params_args) # service backend has NOT been created yet self.assertTrue(paramname not in self.param_if_pool.params.keys()) # create the param and then try updating again, simulating # it coming online after expose call. rospy.set_param(paramname, 'param_value') try: params = self.get_system_state() dt = self.param_if_pool.update(params) self.assertTrue(paramname in dt.added) # paramname added self.assertEqual(dt.removed, []) # nothing removed # every withhold param should STILL be in the list of args self.assertTrue(paramname in self.param_if_pool.params_args) # param backend has been created self.assertTrue(paramname in self.param_if_pool.params.keys()) dt = self.param_if_pool.expose_params([]) self.assertEqual(dt.added, []) # nothing added self.assertTrue(paramname in dt.removed) # paramname removed # every withhold param should NOT be in the list of args self.assertTrue(paramname not in self.param_if_pool.params_args) # param backend should be GONE self.assertTrue(paramname not in self.param_if_pool.params.keys()) params = self.get_system_state() dt = self.param_if_pool.update(params) self.assertEqual(dt.added, []) # nothing added self.assertEqual(dt.removed, []) # nothing removed # every withhold param should STILL NOT be in the list of args self.assertTrue(paramname not in self.param_if_pool.params) # param backend should be GONE self.assertTrue(paramname not in self.param_if_pool.params.keys()) finally: rospy.delete_param(paramname) params = self.get_system_state() dt = self.param_if_pool.update(params) self.assertEqual(dt.added, []) # nothing added self.assertEqual(dt.removed, []) # nonexistent_srv already removed # every withhold service should STILL NOT be in the list of args self.assertTrue(paramname not in self.param_if_pool.params_args) # service backend should be GONE self.assertTrue(paramname not in self.param_if_pool.params.keys())
def __init__(self, context): super(WorkspaceUI, self).__init__(context) self.setObjectName('Instructor Workspace UI') # Create QWidget self._widget = QWidget() # Get path to UI file which is a sibling of this file rospack = rospkg.RosPack() ui_path = rospack.get_path('instructor_core') + '/ui/workspace.ui' # Load the ui attributes into the main widget loadUi(ui_path, self._widget) self._widget.setObjectName('InstructorWorkspaceUI') self._widget.setWindowTitle('Instructor Workspace UI') # Show _widget.windowTitle on left-top of each plugin (when # it's set in _widget). This is useful when you open multiple # plugins at once. Also if you open multiple instances of your # plugin at once, these lines add number to make it easy to # tell from pane to pane. if context.serial_number() > 1: title = self._widget.windowTitle() + (' (%d)' % context.serial_number()) self._widget.setWindowTitle(title) # Add widget to the user interface context.add_widget(self._widget) palette = QPalette() palette.setColor(QPalette.Background, Qt.white) self._widget.setPalette(palette) # Hide things to start with self._widget.fiducial_found_widget.hide() self._widget.add_fiducial_widget.hide() # Connect things self._widget.add_ok_btn.clicked.connect(self.add_ok_event) self._widget.add_cancel_btn.clicked.connect(self.add_cancel_event) self._widget.fiducial_name_edit.textChanged.connect(self.name_entered_event) # Add listener to detect AR markers self.tf_listen = tf.TransformListener() # Add predicator publisher self.pub_predicates = rospy.Publisher('predicator/input', PredicateList) # Clean params for landmarks rospy.loginfo('Cleaning up previous workspace landmark params') params = rospy.get_param_names() landmarks = [p for p in params if "instructor_landmark" in p] rospy.loginfo(landmarks) for marker in landmarks: rospy.loginfo('Removed '+str(marker)) rospy.delete_param(marker) # Add timer self.timer = QtCore.QTimer() self.connect(self.timer, QtCore.SIGNAL("timeout()"), self.update) self.timer.start(100)
def run(): global goal_pub global frontier_pub global getGlobalPath rospy.init_node('exploration_node') navType = rospy.get_param('~nav', default='rbe') if navType != 'rbe': rospy.delete_param('~nav') if navType == 'rbe': print("Using rbe nav") goal_pub = rospy.Publisher('/rbe_goal', PoseStamped, queue_size=1) else: print "using gMapping nav" goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=1) planner.init() nav.init() frontier_pub = rospy.Publisher('/frontier/frontier', GridCells, queue_size=1) getGlobalPath = rospy.ServiceProxy('global_path', CalcPath) # status_sub = rospy.Subscriber('/move_base/status', GoalStatusArray, statusCallback, queue_size=1) map_sub = rospy.Subscriber('/map', OccupancyGrid, mapCallback, queue_size=1) exploreMap()
def main(): example = ExampleMoveItTrajectories() # For testing purposes success = example.is_init_success try: rospy.delete_param("/kortex_examples_test_results/moveit_general_python") except: pass if success: actual_pose = example.get_cartesian_pose() #Try to use the MoveIt function to reach a cartesian pose rospy.loginfo("Reaching cartesian pose...") success &= example.go_to_pose_goal() # rospy.loginfo("Reaching Named Target Vertical...") # success &= example.reach_named_position("vertical") # rospy.loginfo("Reaching Joint Angles...") # success &= example.reach_joint_angles(tolerance=0.01) #rad # rospy.loginfo("Reaching Named Target Home...") # success &= example.reach_named_position("home") # rospy.loginfo("Reaching Cartesian Pose...") actual_pose = example.get_cartesian_pose() actual_pose.position.z += 0.2
def param_talker(): rospy.init_node('param_talker') # Fetch values from the Parameter Server. In this example, we fetch # parameters from three different namespaces: # # 1) global (/global_example) # 2) parent (/foo/utterance) # 3) private (/foo/param_talker/topic_name) # fetch a /global parameter global_example = rospy.get_param("/global_example") rospy.loginfo("%s is %s", rospy.resolve_name('/global_example'), global_example) # fetch the utterance parameter from our parent namespace utterance = rospy.get_param('utterance') rospy.loginfo("%s is %s", rospy.resolve_name('utterance'), utterance) # fetch topic_name from the ~private namespace topic_name = rospy.get_param('~topic_name') rospy.loginfo("%s is %s", rospy.resolve_name('~topic_name'), topic_name) # fetch a parameter, using 'default_value' if it doesn't exist default_param = rospy.get_param('default_param', 'default_value') rospy.loginfo('%s is %s', rospy.resolve_name('default_param'), default_param) # fetch a group (dictionary) of parameters gains = rospy.get_param('gains') p, i, d = gains['P'], gains['I'], gains['D'] rospy.loginfo("gains are %s, %s, %s", p, i, d) # set some parameters rospy.loginfo('setting parameters...') rospy.set_param('list_of_floats', [1., 2., 3., 4.]) rospy.set_param('bool_True', True) rospy.set_param('~private_bar', 1 + 2) rospy.set_param('to_delete', 'baz') rospy.loginfo('...parameters have been set') # delete a parameter if rospy.has_param('to_delete'): rospy.delete_param('to_delete') rospy.loginfo("deleted %s parameter" % rospy.resolve_name('to_delete')) else: rospy.loginfo('parameter %s was already deleted' % rospy.resolve_name('to_delete')) # search for a parameter param_name = rospy.search_param('global_example') rospy.loginfo('found global_example parameter under key: %s' % param_name) # publish the value of utterance repeatedly pub = rospy.Publisher(topic_name, String, queue_size=10) while not rospy.is_shutdown(): pub.publish(utterance) rospy.loginfo(utterance) rospy.sleep(1)
def check_delete_params(self): if self.param_delete: for param in self.param_delete: if (rospy.get_param(param, False)): rospy.delete_param(param) else: self._print_warning("Not deleting any param")
def check_delete_params(self): if self.param_delete: for param in self.param_delete: if (rospy.get_param(param,False)): rospy.delete_param(param) else: self._print_warning("Not deleting any param")
def clean(self): # on shutdown velocity_param_name = rospy.search_param('velocity') if velocity_param_name != None: rospy.delete_param(velocity_param_name)
def set_params(self, m): for ns in self.namespaces: if rospy.has_param(ns): rospy.delete_param(ns) self.set_params_(self.fixed_params) self.set_params_(m) self.scenario.parameterize(m) rospy.set_param('/nav_experiments/scenario', self.scenario.get_scenario())
def reset(self): self.agentMap = {} self.obsMap = {} self.nextObsNum = 60 self.nextAgentNum = 1 self.agentVel = {} rospy.delete_param('/spher_swarm/connected') self.update()
def test_servo_vel(self, time_step): dynamic_pose = self.dynamic_object(step=time_step) dynamic_pose.position.z += 0.1 rospy.set_param('vel_calc','true') self.servo_vel(dynamic_pose, num_wp=2) if rospy.has_param('vel_calc'): rospy.logwarn('Initialized pose.') rospy.delete_param('vel_calc')
def shutdown_all_apps(self): for full_app_name,app_process in self.active_app_launchers_.items(): app_process.terminate() while app_process.poll() == None: pass if rospy.has_param("/wallframe/core/apps/running/" + full_app_name): rospy.delete_param("/wallframe/core/apps/running/" + full_app_name) rospy.logwarn("WallframeAppManager: App [" + full_app_name + "] shutdown successfully") self.active_app_launchers_.clear()
def shutdown(retval): """ We need to delete the parameter that the simulator sets, or else other nodes might still think this node is running. (Maybe there's a better way to do this...) """ rospy.loginfo("Shutting down...") rospy.delete_param(PARAM_NAME) exit(retval)
def param_talker(): rospy.init_node('param_talker') # Fetch values from the Parameter Server. In this example, we fetch # parameters from three different namespaces: # # 1) global (/global_example) # 2) parent (/foo/utterance) # 3) private (/foo/param_talker/topic_name) # fetch a /global parameter global_example = rospy.get_param("/global_example") rospy.loginfo("%s is %s", rospy.resolve_name('/global_example'), global_example) # fetch the utterance parameter from our parent namespace utterance = rospy.get_param('utterance') rospy.loginfo("%s is %s", rospy.resolve_name('utterance'), utterance) # fetch topic_name from the ~private namespace topic_name = rospy.get_param('~topic_name') rospy.loginfo("%s is %s", rospy.resolve_name('~topic_name'), topic_name) # fetch a parameter, using 'default_value' if it doesn't exist default_param = rospy.get_param('default_param', 'default_value') rospy.loginfo('%s is %s', rospy.resolve_name('default_param'), default_param) # fetch a group (dictionary) of parameters gains = rospy.get_param('gains') p, i, d = gains['P'], gains['I'], gains['D'] rospy.loginfo("gains are %s, %s, %s", p, i, d) # set some parameters rospy.loginfo('setting parameters...') rospy.set_param('list_of_floats', [1., 2., 3., 4.]) rospy.set_param('bool_True', True) rospy.set_param('~private_bar', 1+2) rospy.set_param('to_delete', 'baz') rospy.loginfo('...parameters have been set') # delete a parameter if rospy.has_param('to_delete'): rospy.delete_param('to_delete') rospy.loginfo("deleted %s parameter"%rospy.resolve_name('to_delete')) else: rospy.loginfo('parameter %s was already deleted'%rospy.resolve_name('to_delete')) # search for a parameter param_name = rospy.search_param('global_example') rospy.loginfo('found global_example parameter under key: %s'%param_name) # publish the value of utterance repeatedly pub = rospy.Publisher(topic_name, String) while not rospy.is_shutdown(): pub.publish(utterance) rospy.loginfo(utterance) rospy.sleep(1)
def update(self): all_frames = self.listener_.getFrameStrings() ar_frames = [f for f in all_frames if f.find('ar_marker')>=0] un_filtered = [f for f in ar_frames if f.find('filtered')<0] # Decay each frame count at every timestep for f in self.frames.keys(): self.frames[f]['no_frames'] -= 1 if self.frames[f]['no_frames'] <= 0: short_name = 'landmark_' + f.split('_')[len(f.split('_'))-1:][0] rospy.delete_param('instructor_landmark/'+short_name) rospy.logwarn('Deleted:' +short_name) self.frames.pop(f) for frame in un_filtered: try: F = tf_c.fromTf(self.listener_.lookupTransform('/world',frame,rospy.Time(0))) except (tf.LookupException, tf.ConnectivityException) as e: rospy.logerr('Frame ['+frame+'] not found') return except tf.ExtrapolationException as e: # The frame we're seeing is old return if frame not in self.frames.keys(): self.frames[frame] = {'no_frames':0, 'poses':[], 'average_pose':None} rospy.logwarn('New Frame:' + frame) # rospy.logwarn(self.frames[frame]['no_frames']) if self.frames[frame]['no_frames'] < self.buffer: self.frames[frame]['no_frames'] += 2 self.frames[frame]['poses'].append(F) else: self.frames[frame]['poses'].pop(0) self.frames[frame]['poses'].pop(0) self.frames[frame]['poses'].append(F) for frame in self.frames.keys(): # Get all stored frame positions/rotations sum_xyz = [tf_c.toTf(f)[0] for f in self.frames[frame]['poses']] sum_rpy = [f.M.GetRPY() for f in self.frames[frame]['poses']] if len(sum_xyz) > 2: xyz = np.mean(sum_xyz, 0) rpy = np.mean(sum_rpy, 0) F_avg = PyKDL.Frame() F_avg.p = PyKDL.Vector(*xyz) F_avg.M = PyKDL.Rotation.RPY(*rpy) self.broadcaster_.sendTransform(tuple(F_avg.p),tuple(F_avg.M.GetQuaternion()),rospy.Time.now(), '/filtered/'+frame, '/world') # rosparam_marker_name = "instructor_landmark/{}".format(str('filtered/'+frame)) # rospy.set_param(rosparam_marker_name, str('filtered/'+frame)) short_name = 'landmark_' + frame.split('_')[len(frame.split('_'))-1:][0] rospy.set_param('instructor_landmark/'+short_name, str('filtered/'+frame))
def clean_up(self): for app_id,app in self.apps_.items(): if rospy.has_param("/wallframe/core/available_app/" + app_id): rospy.delete_param("/wallframe/core/available_apps/" + app_id) print("App parameters for [" + app_id + "] cleaned up") if rospy.has_param("/wallframe/core/available_apps"): rospy.delete_param("/wallframe/core/available_apps") print("Remaining parameters cleaned up") pass
def shutdown_app(self,app_name): full_app_name = "wallframe_app_" + app_name app_process = self.active_app_launchers_[full_app_name] app_process.terminate() while app_process.poll() == None: pass if rospy.has_param("/wallframe/core/apps/running/" + full_app_name): rospy.delete_param("/wallframe/core/apps/running/" + full_app_name) del self.active_app_launchers_[full_app_name] rospy.logwarn("WallframeAppManager: App [" + full_app_name + "] shutdown successfully")
def delete_param(name, params_glob): if params_glob and not any(fnmatch.fnmatch(str(name), glob) for glob in params_glob): # If the glob list is not empty and there are no glob matches, # stop the attempt to delete the parameter. return # If the glob list is empty (i.e. false) or the parameter matches # one of the glob strings, continue to delete the parameter. if has_param(name, params_glob): with param_server_lock: rospy.delete_param(name)
def set_param(self, param_name, val): # Set private var value setattr(self, '_{}'.format(param_name), val) if val == None: full_param_name = '/needybot/blackboard/{}'.format(param_name) if rospy.has_param(full_param_name): rospy.delete_param(full_param_name) self.publish("delete_{}".format(param_name)) else: rospy.set_param('/needybot/blackboard/{}'.format(param_name), val) self.publish("set_{}".format(param_name))
def remove(self): """ Method which is used to remove the parameter from the parameter server. """ if self._registered: try: rospy.delete_param(self._name) except rospy.ROSException: pass self._registered = False
def on_stopButton_clicked(self): try: rospy.delete_param('/tracking/scenario') except KeyError: pass msg1 = SimControl() msg1.mode = "stop" self.pub_control.publish(msg1) msg2 = Bool() msg2.data = True self.pub_reset.publish(msg2)
def __init__(self, default_update_rate=50.0): rospy.init_node('sphero_swarm') self.update_rate = default_update_rate self.sampling_divisor = int(400/self.update_rate) self.sphero_dict = {} self.sphero_connected = {} self._init_pubsub() self._init_params() self.last_diagnostics_time = rospy.Time.now() self.mutual_lock = threading.Lock() rospy.delete_param('/sphero_swarm/team')
def test_spec3(self): ''' Values within their limits will be rated with 3 (OK). ''' rospy.set_param(spec_namespace, mockSpecs) sh = SpecificationHandler() res = sh.compare(exampleMessages["h!127.0.0.1"], "h!127.0.0.1") self.assertEqual(res.get_value('cpu_temp_stddev')['state'], 3) self.assertEqual(res.get_value('cpu_temp_core')['state'], [3, 3]) try: rospy.delete_param(spec_namespace) except KeyError: pass
def delete_map(self, request): rospy.logdebug("Service call : delete map %s" % request.map_id) response = DeleteMapResponse() if rospy.has_param('~last_map_id') and rospy.get_param('~last_map_id') == request.map_id: rospy.delete_param('~last_map_id') if self.last_map == request.map_id: self.last_map = None if self.map_collection.remove({'uuid': {'$in': [request.map_id]}}) == 0: return None return response
def test_spec2(self): ''' Values without specifications will be rated with 2 (OK). ''' rospy.set_param(spec_namespace, mockSpecs) sh = SpecificationHandler() res = sh.compare(exampleMessages["h!127.0.0.1"], "h!127.0.0.1") self.assertEqual(res.get_value('ram_usage_max')['state'], 2) self.assertEqual(res.get_value('cpu_usage_core_max')['state'], [2, 2]) try: rospy.delete_param(spec_namespace) except KeyError: pass
def remote_destroy(self): """ Method should be called to delete the Parameter from the parameter server. """ if self._registered: try: rospy.delete_param(self._name) except rospy.ROSException: pass self._registered = False if self._owner: self._owner.unregisterParameter(self) self._owner = None
def test_spec10(self): ''' Values below their limits will be rated with 1 (Low). Values above their limits will be rated with 0 (High). ''' rospy.set_param(spec_namespace, mockSpecs) msg = exampleMessages["h!127.0.0.1"] msg.cpu_temp_core_mean[0] -= 30 msg.cpu_temp_core_mean[1] += 10 sh = SpecificationHandler() res = sh.compare(msg, "h!127.0.0.1") self.assertEqual(res.get_value('cpu_temp_core_mean')['state'], [1, 0]) try: rospy.delete_param(spec_namespace) except KeyError: pass
def handle_msg(msg, who): assert isinstance(msg, Odometry) global last_cap_r, last_cap_f, last_cap_yaw, current_time now = rospy.get_rostime() if now<current_time : reset() try: rospy.delete_param('obs_centroid') except KeyError: pass current_time = now if who == 'cap_r': last_cap_r = rtk_position_to_numpy(msg) elif who == 'cap_f' and last_cap_r is not None: cap_f = rtk_position_to_numpy(msg) cap_r = last_cap_r last_cap_f = cap_f last_cap_yaw = get_yaw(cap_f, cap_r) elif who == 'obs_r' and last_cap_f is not None and last_cap_yaw is not None: md = None for obs in metadata: if obs['obstacle_name'] == 'obs1': md = obs assert md, 'obs1 metadata not found' # find obstacle rear RTK to centroid vector lrg_to_gps = [md['gps_l'], -md['gps_w'], md['gps_h']] lrg_to_centroid = [md['l'] / 2., -md['w'] / 2., md['h'] / 2.] obs_r_to_centroid = np.subtract(lrg_to_centroid, lrg_to_gps) # in the fixed GPS frame cap_f = last_cap_f obs_r = rtk_position_to_numpy(msg) # in the capture vehicle front RTK frame cap_to_obs = np.dot(rotMatZ(-last_cap_yaw), obs_r - cap_f) cap_to_obs_centroid = cap_to_obs + obs_r_to_centroid velo_to_front = np.array([-1.0922, 0, -0.0508]) cap_to_obs_centroid += velo_to_front rospy.set_param('obs_centroid', cap_to_obs_centroid.tolist())
def main(args): parser = argparse.ArgumentParser() parser.add_argument('params', metavar='param', nargs='+', help='Parameters to delete on shutdown') args = parser.parse_args(rospy.myargv()[1:]) rospy.init_node('on_shutdown_param_cleaner', anonymous=True) print('Will delete params %s on shutdown' % args.params) try: while not rospy.is_shutdown(): rospy.sleep(1) except rospy.exceptions.ROSInterruptException: pass for param in args.params: if rospy.has_param(param): rospy.delete_param(param)
def stop(self): print "stop sphero swarm" #tell the ball to stop moving before quiting for name in self.sphero_dict: sphero = self.sphero_dict[name] sphero.robot.roll(int(0), int(0), 1, False) sphero.robot.shutdown = True rospy.sleep(1.0) for name in self.sphero_dict: sphero = self.sphero_dict[name] sphero.is_connected = sphero.robot.disconnect() sphero.robot.join() del self.sphero_dict[name] del self.team_info[name] del self.sphero_connected[name] #rospy.set_param('/sphero_swarm/team', self.team_info) rospy.delete_param('/sphero_swarm/team') rospy.set_param('/sphero_swarm/connected', self.sphero_connected)