我们从Python开源项目中，提取了以下50个代码示例，用于说明如何使用rospy.Service()。
def __init__(self): # first let's load all parameters: self.frame_id = rospy.get_param("~frame_id", "odom_meas") self.camera_frame_id = rospy.get_param("~camera_frame_id", "overhead_cam_frame") self.base_frame_id = rospy.get_param("~base_frame_id", "base_meas") self.x0 = rospy.get_param("~x0", 0.0) self.y0 = rospy.get_param("~y0", 0.0) self.th0 = rospy.get_param("~th0", 0.0) self.pubstate = rospy.get_param("~pubstate", True) self.marker_id = rospy.get_param("~marker_id", 12) # setup other required vars: self.odom_offset = odom_conversions.numpy_to_odom(np.array([self.x0, self.y0, self.th0]), self.frame_id) self.path_list = deque([], maxlen=PATH_LEN) # now let's create publishers, listeners, and subscribers self.br = tf.TransformBroadcaster() self.listener = tf.TransformListener() self.meas_pub = rospy.Publisher("meas_pose", Odometry, queue_size=5) self.path_pub = rospy.Publisher("meas_path", Path, queue_size=1) self.alvar_sub = rospy.Subscriber("ar_pose_marker", AlvarMarkers, self.alvarcb) self.publish_serv = rospy.Service("publish_bool", SetBool, self.pub_bool_srv_cb) self.offset_serv = rospy.Service("set_offset", SetOdomOffset, self.offset_srv_cb) return
def run(self): self.service_type, self.service_args = wait_service_ready(self.service_name, self.url) if not self.service_type: rospy.logerr('Type of service %s are not equal in the remote and local sides', self.service_type) return service_type_module, service_type_name = tuple(self.service_type.split('/')) try: roslib.load_manifest(service_type_module) msg_module = import_module(service_type_module + '.srv') self.srvtype = getattr(msg_module, service_type_name) if self.test: self.caller = rospy.Service(self.service_name + '_rb', self.srvtype, self.callback)#, self.queue_size) else: self.caller = rospy.Service(self.service_name, self.srvtype, self.callback)#, self.queue_size) self.ws = websocket.WebSocketApp(self.url, on_message = self.on_message, on_error = self.on_error, on_close = self.on_close, on_open = self.on_open) rospy.loginfo('Create connection to Rosbridge server %s for calling service %s successfully', self.url, self.service_name) self.ws.run_forever() except ResourceNotFound, e: rospy.logerr('Proxy for service %s init falied. Reason: Could not find the required resource: %s', self.service_name, str(e)) except Exception, e: rospy.logerr('Proxy for service %s init falied. Reason: %s', self.service_name, str(e))
def __init__(self): self.calibrator = HandeyeCalibrator() self.get_sample_list_service = rospy.Service(hec.GET_SAMPLE_LIST_TOPIC, hec.srv.TakeSample, self.get_sample_lists) self.take_sample_service = rospy.Service(hec.TAKE_SAMPLE_TOPIC, hec.srv.TakeSample, self.take_sample) self.remove_sample_service = rospy.Service(hec.REMOVE_SAMPLE_TOPIC, hec.srv.RemoveSample, self.remove_sample) self.compute_calibration_service = rospy.Service(hec.COMPUTE_CALIBRATION_TOPIC, hec.srv.ComputeCalibration, self.compute_calibration) self.save_calibration_service = rospy.Service(hec.SAVE_CALIBRATION_TOPIC, std_srvs.srv.Empty, self.save_calibration) # Useful for secondary input sources (e.g. programmable buttons on robot) self.take_sample_topic = rospy.Subscriber(hec.TAKE_SAMPLE_TOPIC, std_msgs.msg.Empty, self.take_sample) self.compute_calibration_topic = rospy.Subscriber(hec.REMOVE_SAMPLE_TOPIC, std_msgs.msg.Empty, self.remove_last_sample) # TODO: normalize self.last_calibration = None
def service(self, *upper_args, **kwargs): if isinstance(upper_args[0], str): service_name, srv_type = upper_args def __decorator(func): def __inner(request): n_args = func.func_code.co_argcount if "self" in func.func_code.co_varnames[:n_args]: return self.__class_service(func, request, service_name) else: return self.__function_service(func, request, service_name) args = [service_name, srv_type, __inner] service = rospy.Service(*args, **kwargs) self.services.append(service) return __inner return __decorator else: raise ValueError("First argument to service must be service name as str")
def __init__(self): rospy.init_node('fake_renderer', anonymous=True) try: topic_name = rospy.get_param('~action_name') except KeyError as e: print('[ERROR] Needed parameter for (topic_name)...') quit() if 'render_gesture' in rospy.get_name(): self.GetInstalledGesturesService = rospy.Service( "get_installed_gestures", GetInstalledGestures, self.handle_get_installed_gestures ) self.motion_list = { 'neutral': ['neutral_motion1'], 'encourge': ['encourge_motion1'], 'attention': ['attention_motion1'], 'consolation': ['consolation_motion1'], 'greeting': ['greeting_motion1'], 'waiting': ['waiting_motion1'], 'advice': ['advice_motion1'], 'praise': ['praise_motion1'], 'command': ['command_motion1'], } self.server = actionlib.SimpleActionServer( topic_name, RenderItemAction, self.execute_callback, False) self.server.start() rospy.loginfo('[%s] initialized...' % rospy.get_name()) rospy.spin()
def register_services(self, services): """ Similar to `pub_sub.create_publishers` and `pub_sub.create_subscribers`, this method takes a list of service definition pairs, and registers a `rospy.Service` object for each. Because services must be uniquely namespaced, each service will be exposed externally using the class's namespace as defined at construction time. This means services map as follows /self.namespace_method -> self.method(req) Each pair consists of the name of the service and its service type, as defined by a .srv file. """ for service in services: callback_name, service_type = service service_name = '{}_{}'.format(self.namespace, callback_name) callback = getattr(self, callback_name) self.registered.append(rospy.Service(service_name, service_type, callback))
def ROS_Subscribe(self): def handler(event): uavcan_req = canros.copy_ros_uavcan(self.UAVCAN_Type.Request(), event, request=True) q = Queue(maxsize=1) def callback(event): q.put(event.response if event else None) uavcan_id = getattr(event, canros.uavcan_id_field_name) if uavcan_id == 0: return uavcan_node.request(uavcan_req, uavcan_id, callback, timeout=1) # Default UAVCAN service timeout is 1 second uavcan_resp = q.get() if uavcan_resp is None: return return canros.copy_uavcan_ros(self.Response_Type(), uavcan_resp, request=False) self.Service(handler)
def request(self, uavcan_msg, node_id, callback, priority=None, timeout=None): #pylint: disable=W0613 uavcan_type = uavcan.get_uavcan_data_type(uavcan_msg) if not uavcan_type.full_name in self.__service_proxies: self.__service_proxies[uavcan_type.full_name] = Service(uavcan_type.full_name).ServiceProxy() service_proxy = self.__service_proxies[uavcan_type.full_name] ros_req = copy_uavcan_ros(service_proxy.request_class(), uavcan_msg, request=True) setattr(ros_req, uavcan_id_field_name, node_id) def service_proxy_call(): try: return service_proxy(ros_req) except rospy.ServiceException: return None def request_finished(fut): ros_resp = fut.result() if ros_resp is None: uavcan_resp = None else: uavcan_resp = uavcan_node._Event(self, uavcan_type, ros_resp, uavcan_id=node_id, request=False) callback(uavcan_resp) self.__request_executor.submit(service_proxy_call).add_done_callback(request_finished)
def save(self, i_save, action, endeffector_pose): self.t_savereq = rospy.get_time() assert self.instance_type == 'main' if self.use_aux: # request save at auxiliary recorders try: rospy.wait_for_service('get_kinectdata', 0.1) resp1 = self.save_kinectdata_func(i_save) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) raise ValueError('get_kinectdata service failed') if self.save_images: self._save_img_local(i_save) if self.save_actions: self._save_state_actions(i_save, action, endeffector_pose) if self.save_gif: highres = cv2.cvtColor(self.ltob.img_cv2, cv2.COLOR_BGR2RGB) print 'highres dim',highres.shape self.highres_imglist.append(highres)
def wait_for_human_interaction(self, arm_threshold=1, joystick_threshold=0.15): rospy.loginfo("We are waiting for human interaction...") def detect_arm_variation(): new_effort = np.array(self.topics.torso_l_j.effort) delta = np.absolute(effort - new_effort) return np.amax(delta) > arm_threshold def detect_joy_variation(): return np.amax(np.abs(self.topics.joy1.axes)) > joystick_threshold effort = np.array(self.topics.torso_l_j.effort) rate = rospy.Rate(50) is_joystick_demo = None while not rospy.is_shutdown(): if detect_arm_variation(): is_joystick_demo = False break elif detect_joy_variation(): is_joystick_demo = True break rate.sleep() return is_joystick_demo ################################# Service callbacks
def __init__(self): self.rospack = RosPack() with open(join(self.rospack.get_path('apex_playground'), 'config', 'general.json')) as f: self.params = json.load(f) with open(join(self.rospack.get_path('apex_playground'), 'config', 'torso.json')) as f: self.torso_params = json.load(f) self.outside_ros = rospy.get_param('/use_sim_time', False) # True if work manager <-> controller comm must use ZMQ id = search(r"(\d+)", rospy.get_namespace()) self.worker_id = 0 if id is None else int(id.groups()[0]) # TODO string worker ID self.work = WorkManager(self.worker_id, self.outside_ros) self.torso = Torso() self.ergo = Ergo() self.learning = Learning() self.perception = Perception() self.recorder = Recorder() self.demonstrate = "" # Skill (Target space for Produce) or empty string if not running assessment # Served services self.service_name_demonstrate = "controller/assess" rospy.Service(self.service_name_demonstrate, Assess, self.cb_assess) rospy.loginfo('Controller fully started!')
def _init_pubsub(self): self.joint_states_pub = rospy.Publisher('joint_states', JointState) self.odom_pub = rospy.Publisher('odom', Odometry) self.sensor_state_pub = rospy.Publisher('~sensor_state', TurtlebotSensorState) self.operating_mode_srv = rospy.Service('~set_operation_mode', SetTurtlebotMode, self.set_operation_mode) self.digital_output_srv = rospy.Service('~set_digital_outputs', SetDigitalOutputs, self.set_digital_outputs) if self.drive_mode == 'twist': self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Twist, self.cmd_vel) self.drive_cmd = self.robot.direct_drive elif self.drive_mode == 'drive': self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Drive, self.cmd_vel) self.drive_cmd = self.robot.drive elif self.drive_mode == 'turtle': self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Turtle, self.cmd_vel) self.drive_cmd = self.robot.direct_drive else: rospy.logerr("unknown drive mode :%s"%(self.drive_mode)) self.transform_broadcaster = None if self.publish_tf: self.transform_broadcaster = tf.TransformBroadcaster()
def get_active_mission(req): """ Service to update mission information with current active mission. Args: req: Request of type Trigger. Returns: TriggerResponse with true, false for success, failure. """ with lock: global msgs try: msgs = client.get_active_mission(frame) except (ConnectionError, Timeout) as e: rospy.logwarn(e) return False, str(e) except (ValueError, HTTPError) as e: rospy.logerr(e) return False, str(e) rospy.loginfo("Using active mission") return True, "Success"
def __init__(self, config): host = unrealcv.HOST port = unrealcv.PORT if 'endpoint' in config: host, port = config['endpoint'] if 'port' in config: port = config['port'] if 'hostname' in config: host = config['hostname'] self.opencv_bridge = cv_bridge.CvBridge() self._client_lock = threading.Lock() self._client = unrealcv.Client(endpoint=(host, port)) self._client.connect() if not self._client.isconnected(): raise RuntimeError("Could not connect to unrealcv simulator, is it running?") # Service attributes self._get_camera_view_service = None
def __init__(self, name, color=None): GazeboObject.__init__(self, name) self.name = name self.publisher = rospy.Publisher('/'+name+'/lamp/visual/set_color', MaterialColor, queue_size=1) self.color = None self._on = False if color: self.set_color(color) self._send_color_cmd(Light.off_color) # This is of course useful only if the python object is being asociated with an already spawend model. Otherwise, it just does nothing # self._get_light_service = rospy.Service('/env/'+name+'/lamp/visual/get_state', # GetLightState, # self.get_light_state_callback) self._set_light_service = rospy.Service('/env/'+name+'/lamp/visual/set_state', SetLightState, self.set_light_state_callback) thread = threading.Thread(target=self.publish_state, args=()) thread.daemon = True # Daemonize thread thread.start() # Start the execution # def get_light_state_callback(self, req): # return GetLightStateResponse(self._on)
def trigger_configuration(self): """ Callback when the configuration button is clicked """ srv_name, ok = QInputDialog.getText(self._widget, "Select service name", "Service name") if ok: self._create_service_server(srv_name)
def _create_service_server(self, srv_name): """ Method that creates a service server for a Recognize.srv :param srv_name: """ if self._srv: self._srv.shutdown() if srv_name: rospy.loginfo("Creating service '%s'" % srv_name) self._srv_name = srv_name self._srv = rospy.Service(srv_name, Recognize, self.recognize_srv_callback)
def __init__(self): rospy.init_node('memory_node', anonymous=False) while not rospy.is_shutdown(): try: port = rospy.get_param('/social_mind/port') host = rospy.get_param('/social_mind/host') break except KeyError as e: rospy.loginfo('[%s] wait for bringup social_mind node...'%rospy.get_name()) rospy.sleep(1.0) continue try: self.client = pymongo.MongoClient(host, port, serverSelectionTimeoutMS=2000) self.client.is_mongos # Wait for connection self.db = self.client[rospy.get_param('~name_data_group')] # self.db.set_profiling_level(0, 400) # Performance Setting: Set the database’s profiling level to 0, and slow_ms is 400ms. self.collections = {} self.data_template = {} except pymongo.errors.ServerSelectionTimeoutError, e: rospy.logerr('Error: %s'%e.message) quit() except KeyError, e: quit() self.srv_read_data = rospy.Service('%s/read_data'%rospy.get_name(), ReadData, self.handle_read_data) self.srv_write_data = rospy.Service('%s/write_data'%rospy.get_name(), WriteData, self.handle_write_data) self.srv_register_data = rospy.Service('%s/register_data'%rospy.get_name(), RegisterData, self.handle_register_data) self.srv_get_data_list = rospy.Service('%s/get_data_list'%rospy.get_name(), GetDataList, self.handle_get_data_list) self.wait_event_server = actionlib.SimpleActionServer('%s/wait_event'%rospy.get_name(), WaitEventAction, self.handle_wait_event, auto_start=False) self.wait_event_server.start() rospy.loginfo('[%s] initialzed and ready to use...'%rospy.get_name())
def register_services(self): """Register services for instance""" rospy.Service(services.START_RECIPE, StartRecipe, self.start_recipe_service) rospy.Service(services.STOP_RECIPE, Empty, self.stop_recipe_service) rospy.set_param( params.SUPPORTED_RECIPE_FORMATS, ','.join(RECIPE_INTERPRETERS.keys()) ) return self
def __init__(self): self.state = [lambda: pass_ball_first.pass_first, lambda: pass_ball_second.pass_second, lambda: pass_ball_third.pass_third, lambda: shoot_ball_first.shoot_first, lambda: shoot_ball_second.shoot_second , lambda: shoot_ball_third.shoot_third ] self.service = rospy.Service('Control_State', basketball_srv.ControlState, self.srv_callback)
def __init__(self, uavcan_type): super(Service, self).__init__(uavcan_type) self.__ros_service_proxy = None # This is the canros server so the service naming scheme is the reverse of the canros API.
def Request_Name(self): return super(Service, self).Response_Name
def Response_Name(self): return super(Service, self).Request_Name
def Request_Type(self): return super(Service, self).Response_Type
def Response_Type(self): return super(Service, self).Request_Type
def Response_Topic(self): return super(Service, self).Request_Topic
def Service(self, handler, **kw): return rospy.Service(self.Response_Topic, self.Type, handler, **kw) # Converts a string to a list of uint8s.
def ros_type_from_type(uavcan_type): if uavcan_type.category == uavcan_type.CATEGORY_COMPOUND: if uavcan_type.kind == uavcan_type.KIND_MESSAGE: return Message(uavcan_type.full_name).Type elif uavcan_type.kind == uavcan_type.KIND_SERVICE: return Service(uavcan_type.full_name).Type # No ROS type so return something that evaluates to None. return lambda: None
def __init__(self, align_path, net_path, storage_folder): self._bridge = CvBridge() self._learn_srv = rospy.Service('learn', LearnFace, self._learn_face_srv) self._detect_srv = rospy.Service('detect', DetectFace, self._detect_face_srv) self._clear_srv = rospy.Service('clear', Empty, self._clear_faces_srv) # Init align and net self._align = openface.AlignDlib(align_path) self._net = openface.TorchNeuralNet(net_path, imgDim=96, cuda=False) self._face_detector = dlib.get_frontal_face_detector() self._face_dict = {} # Mapping from string to list of reps self._face_dict_filename = rospy.get_param( '~face_dict_filename', '' ) if ( self._face_dict_filename != '' ): if ( not os.path.isfile( self._face_dict_filename ) ): print '_face_dict does not exist; will save to %s' % self._face_dict_filename else: with open( self._face_dict_filename, 'rb') as f: self._face_dict = pickle.load( f ) print 'read _face_dict: %s' % self._face_dict_filename if not os.path.exists(storage_folder): os.makedirs(storage_folder) self._storage_folder = storage_folder
def get_service_handler(name): """ :type name: str, object :rtype: ServiceHandler """ if not isinstance(name, str): name = name.__name__ try: return service_handlers[name] except KeyError: raise ValueError("Service with name {} is not implemented!".format(name))
def register_service_raw(self, callback): """ Registers the service with a callback function. :param callback: callback function to be executed when the service is called. :return: service object. """ return rospy.Service(self.SERVICE_CHANNEL, self.service_class, callback)
def robot_inference_server(): rospy.init_node('robot_inference_server') s = rospy.Service('inference', Inference, handle) print ("Ready to infer robot behavior.") rospy.spin()
def init_traj(self, itr): assert self.instance_type == 'main' # request init service for auxiliary recorders if self.use_aux: try: rospy.wait_for_service('init_traj', timeout=1) resp1 = self.init_traj_func(itr, self.igrp) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) raise ValueError('get_kinectdata service failed') self._init_traj_local(itr) if ((itr+1) % self.ngroup) == 0: self.igrp += 1
def delete_traj(self, tr): assert self.instance_type == 'main' if self.use_aux: try: rospy.wait_for_service('delete_traj', 0.1) resp1 = self.delete_traj_func(tr, self.igrp) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) raise ValueError('delete traj service failed') self._delete_traj_local(tr)
def get_aux_img(self): try: rospy.wait_for_service('get_kinectdata', 0.1) resp1 = self.get_kinectdata_func() self.ltob_aux1.img_msg = resp1.image except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) raise ValueError('get_kinectdata service failed')
def run(self): self.reset_srv = rospy.Service(self.reset_srv_name, Reset, self._cb_reset) self.go_to_start() if self.demo: self.torso.start_idle_motion('head') self.torso.start_idle_motion('right_arm') rospy.spin() if self.demo: self.torso.stop_idle_motion('head') self.torso.stop_idle_motion('right_arm')
def run(self): for service in [self.service_name_set_compliant]: rospy.loginfo("Perception is waiting service {}".format(service)) rospy.wait_for_service(service) self.set_torso_compliant_srv = rospy.ServiceProxy(self.service_name_set_compliant, SetTorsoCompliant) rospy.Service(self.service_name_get, GetSensorialState, self.cb_get) rospy.Service(self.service_name_record, Record, self.cb_record) rospy.loginfo("Done, perception is up!") rospy.spin()
def __init__(self): self.rospack = RosPack() with open(join(self.rospack.get_path('apex_playground'), 'config', 'ergo.json')) as f: self.params = json.load(f) self.button = Button(self.params) self.rate = rospy.Rate(self.params['publish_rate']) self.button.switch_led(False) # Service callers self.robot_reach_srv_name = '{}/reach'.format(self.params['robot_name']) self.robot_compliant_srv_name = '{}/set_compliant'.format(self.params['robot_name']) rospy.loginfo("Ergo node is waiting for poppy controllers...") rospy.wait_for_service(self.robot_reach_srv_name) rospy.wait_for_service(self.robot_compliant_srv_name) self.reach_proxy = rospy.ServiceProxy(self.robot_reach_srv_name, ReachTarget) self.compliant_proxy = rospy.ServiceProxy(self.robot_compliant_srv_name, SetCompliant) rospy.loginfo("Controllers connected!") self.state_pub = rospy.Publisher('ergo/state', CircularState, queue_size=1) self.button_pub = rospy.Publisher('sensors/buttons/help', Bool, queue_size=1) self.goals = [] self.goal = 0. self.joy_x = 0. self.joy_y = 0. self.motion_started_joy = 0. self.js = JointState() rospy.Subscriber('sensors/joystick/{}'.format(self.params["control_joystick_id"]), Joy, self.cb_joy) rospy.Subscriber('{}/joint_state'.format(self.params['robot_name']), JointState, self.cb_js) rospy.Subscriber('sensors/button_leds/pause', Bool, self.cb_bt_led) self.t = rospy.Time.now() self.srv_reset = None self.extended = False self.standby = False self.last_activity = rospy.Time.now() self.delta_t = rospy.Time.now()
def run_within_ros(self): rospy.logwarn('Work Manager is using ROS to distribute work') # Use ROS for work manager <-> controller comm rospy.Service('work/get', GetWork, lambda req: GetWorkResponse(**self._cb_get_work(req.worker))) rospy.Service('work/update', UpdateWorkStatus, lambda req: UpdateWorkStatusResponse(**self._cb_update_work(req.task, req.trial, req.worker, req.iteration, req.success))) rospy.Service('work/add', AddWork, self._cb_add_work) rospy.spin()
def run(self): rospy.Service(self.service_name_perceive, Perceive, self.cb_perceive) rospy.Service(self.service_name_produce, Produce, self.cb_produce) rospy.Service(self.service_name_set_interest, SetFocus, self.cb_set_focus) rospy.Service(self.service_name_set_iteration, SetIteration, self.cb_set_iteration) rospy.Service(self.service_name_interests, GetInterests, self.cb_get_interests) rospy.loginfo("Learning is up!") rate = rospy.Rate(self.params['publish_rate']) while not rospy.is_shutdown(): self.publish() rate.sleep()
def publish(self): if self.learning is not None: with self.lock_iteration: focus = copy(self.focus) ready = copy(self.ready_for_interaction) self.pub_ready.publish(Bool(data=ready)) self.pub_user_focus.publish(String(data=focus if focus is not None else "")) self.pub_focus.publish(String(data=self.learning.get_last_focus())) self.pub_iteration.publish(UInt32(data=self.learning.get_iterations())) ################################# Service callbacks
def run(self): rospy.Service('recorder/record', RecordScene, self.cb_record) rospy.spin() cv2.destroyAllWindows()
def get_mission_by_id(req): """ Service to update mission information with specific mission as given by id. Args: req: GetMissionByID type request with field id corresponding to the mission Returns: GetMissionByIdResponse which is true for success and false for failure. """ with lock: global msgs try: msgs = client.get_mission(req.id, frame) except (ConnectionError, Timeout) as e: rospy.logwarn(e) return False, str(e) except (ValueError, HTTPError) as e: rospy.logerr(e) return False, str(e) except Exception as e: rospy.logfatal(e) return False, str(e) rospy.loginfo("Using mission ID: %d", req.id) return True, "Success"
def __init__(self): super(Organizer, self).__init__() rospy.Service("organizer/topic", AdvertiseUDP, self.__advertise_callback) #self.udptopics_pub = rospy.Publisher("organizer/udptopics", TopicInfoArray, queue_size=1) #self.topics.topics = []
def __init__(self, listenInt, interface, freq, essid, psswd, ip, nm, discoverOnce=True): self.robotName = os.getenv('HOSTNAME') self.tolerance = 20 self.x = 0 self.y = 0 self.client = WiFiTrilatClient() self.discoverOnce = discoverOnce rospy.init_node(self.robotName + "_wifitrilat_server") #self.rssiPub = rospy.Publisher('/' + self.robotName + '/WiFiRSSI', WiFiRSSIMsg, queue_size=10) self.service = rospy.Service("/" + self.robotName + "/WiFiTrilat", WiFiTrilat, self.handle_Trilat) self.listenInt = listenInt self.interface = interface #self.mac = mac.lower() self.freq = int(freq) self.msgs = [] cli.execute_shell("ifconfig %s down" % self.listenInt) #self.wifi = Wireless(self.interface).setFrequency("%.3f" % (float(self.freq) / 1000)) self.connectToNet(essid, psswd,ip, nm) cli.execute_shell("ifconfig %s up" % self.listenInt) self.purge = rospy.Timer(rospy.Duration(2), self.msgPurge) self.heartbeat = rospy.Timer(rospy.Duration(1), self.heartbeat_call) self.discoverTimer = rospy.Timer(rospy.Duration(20), self.findSelfPos) sniff(iface=self.listenInt, prn=self.handler, store=0) rospy.spin()
def __init__(self,side,ik=True): limb.Limb.__init__(self,side) self.side=side self.DEFAULT_BAXTER_SPEED=0.3 if not side in ["left","right"]: raise BaxterException,"Error non existing side: %s, please provide left or right"%side self.post=Post(self) self.__stop = False self.simple=self self._moving=False self.moving_lock=Lock() self.ik=ik if self.ik: self.ns = "/ExternalTools/%s/PositionKinematicsNode/IKService"%self.side rospy.loginfo("Waiting for inverse kinematics service on %s..."%self.ns) rospy.wait_for_service(self.ns) self.iksvc = rospy.ServiceProxy(self.ns, SolvePositionIK) rospy.loginfo("Waiting for inverse kinematics service DONE") else: rospy.loginfo("Skipping inverse kinematics service loading") #self.simple_limb_srv = rospy.Service("/simple_limb/"+side,LimbPose,self.__cbLimbPose) self._pub_speed=rospy.Publisher("/robot/limb/%s/set_speed_ratio"%self.side,Float64,queue_size=1) while not rospy.is_shutdown() and self._pub_speed.get_num_connections() < 1: rospy.sleep(0.1) #self.setSpeed(3) # def __cbLimbPose(self,msg): # cmd = msg.command # if cmd == "goToPose": # resp = self.goToPose(msg.pose, msg.speed, msg.position_tolerance, msg.orientation_tolerance, msg.joint_tolerance, msg.speed_tolerance, msg.timeout, cartesian, msg.path_tolerance) # elif cmd=="goToAngles": # resp = self.goToAngles(msg.angles, msg.speed, msg.joint_tolerance_plan, msg.joint_tolerance, msg.speed_tolerance, msg.timeout, msg.path_tolerance) # return LimbPoseResponse(resp)
def getAnglesFromPose(self,pose): if type(pose)==Pose: goal=PoseStamped() goal.header.frame_id="/base" goal.pose=pose else: goal=pose if not self.ik: rospy.logerror("ERROR: Inverse Kinematics service was not loaded") return None goalstr="%f,%f,%f"%(goal.pose.position.x,goal.pose.position.y,goal.pose.position.z) ikreq = SolvePositionIKRequest() ikreq.pose_stamp.append(goal) try: resp = self.iksvc(ikreq) if (resp.isValid[0]): return resp.joints[0] else: rospy.logerr("FAILURE - No Valid Joint Solution Found for %s"%goalstr) return None except rospy.ServiceException,e : rospy.loginfo("Service call failed: %s" % (e,)) return None
def get_service(self, service_name, service_class, callback): return rospy.Service(service_name, service_class, callback)
def create_services(self): print("Starting services...") # Camera control services self._services.append(rospy.Service('get_camera_view', services.GetCameraImage, self.handle_get_camera_image)) self._services.append(rospy.Service('get_camera_view_with_filename', services.GetCameraImageWithFilename, self.handle_get_camera_image)) self._services.append(rospy.Service('get_camera_location', services.GetCameraLocation, self.handle_get_camera_location)) self._services.append(rospy.Service('get_camera_rotation', services.GetCameraRotation, self.handle_get_camera_rotation)) self._services.append(rospy.Service('get_viewmode', services.GetViewmode, self.handle_get_viewmode)) self._services.append(rospy.Service('move_camera', services.MoveCamera, self.handle_move_camera)) self._services.append(rospy.Service('set_camera_location', services.SetCameraLocation, self.handle_set_camera_location)) self._services.append(rospy.Service('set_camera_rotation', services.SetCameraRotation, self.handle_set_camera_rotation)) self._services.append(rospy.Service('set_viewmode', services.SetViewmode, self.handle_set_viewmode)) # object control services self._services.append(rospy.Service('get_object_color', services.GetObjectColor, self.handle_get_object_color)) self._services.append(rospy.Service('set_object_color', services.SetObjectColor, self.handle_set_object_color)) self._services.append(rospy.Service('get_object_location', services.GetObjectLocation, self.handle_get_object_location)) self._services.append(rospy.Service('get_object_rotation', services.GetObjectRotation, self.handle_get_object_rotation)) self._services.append(rospy.Service('set_object_location', services.SetObjectLocation, self.handle_set_object_location)) self._services.append(rospy.Service('set_object_rotation', services.SetObjectRotation, self.handle_set_object_rotation))
def get_camera_image(self, camera_id, location, rotation): roll, pitch, yaw = rotation self._client_lock.acquire() self._client.request("vset /camera/{0}/location {1} {2} {3}".format(camera_id, location[0], location[1], location[2])) self._client.request("vset /camera/{0}/rotation {1} {2} {3}".format(camera_id, pitch, yaw, roll)) image_filename = self._client.request("vget /camera/{0}/lit".format(camera_id)) self._client_lock.release() return image_filename # Service Handlers
