我们从Python开源项目中，提取了以下50个代码示例，用于说明如何使用rospy.ServiceProxy()。
def ik_solve(limb, pos, orient): ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') poses = { str(limb): PoseStamped(header=hdr, pose=Pose(position=pos, orientation=orient))} ikreq.pose_stamp.append(poses[limb]) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) return 1 if resp.isValid[0]: limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position)) return limb_joints else: return Errors.RaiseNotReachable(pos)
def __init__(self): rospy.init_node('multiplexer_node', anonymous=False) rospy.loginfo('\033[93m[%s]\033[0m wait for bringup social_events_memory...'%rospy.get_name()) rospy.wait_for_service('social_events_memory/read_data') self.rd_event_mem = rospy.ServiceProxy('social_events_memory/read_data', ReadData) rospy.Subscriber('forwarding_event', ForwardingEvent, self.handle_social_events) self.pub_rasing_event = rospy.Publisher('raising_events', RaisingEvents, queue_size=10) self.events_queue = Queue.Queue() self.recognized_words_queue = Queue.Queue() event_period = rospy.get_param('~event_period', 0.5) rospy.Timer(rospy.Duration(event_period), self.handle_trigger_events) rospy.loginfo('\033[93m[%s]\033[0m initialized...'%rospy.get_name()) rospy.spin()
def __init__(self, parent_window): # Service Names. self.service_names = self.get_service_names() self.main_label = Label(parent_window, text="Reset Rovio (NPOSE = 0)", font="Times 14 bold") self.main_label.grid(row=0, columnspan=2) # Init rovio button. current_row = 1 self.init_rovio_button = Button(parent_window, text = 'Reset Rovio', command = self.init_rovio_button_handler) self.init_rovio_button.grid(row = current_row, columnspan = 2) # Init rovio service message. current_row = 1 self.init_message_label = Label(parent_window, text ='', wraplength = 450) self.init_message_label.grid(row = current_row, columnspan = 2) # Init rovio service client. self.init_rovio_srv = rospy.ServiceProxy(self.service_names['init_rovio'], std_srvs.srv.Trigger)
def __init__(self): self.eye_on_hand = rospy.get_param('eye_on_hand', False) # tf names self.robot_base_frame = rospy.get_param('robot_base_frame', 'base_link') self.robot_effector_frame = rospy.get_param('robot_effector_frame', 'tool0') self.tracking_base_frame = rospy.get_param('tracking_base_frame', 'optical_origin') self.tracking_marker_frame = rospy.get_param('tracking_marker_frame', 'optical_target') rospy.wait_for_service(hec.GET_SAMPLE_LIST_TOPIC) self.get_sample_proxy = rospy.ServiceProxy(hec.GET_SAMPLE_LIST_TOPIC, hec.srv.TakeSample) rospy.wait_for_service(hec.TAKE_SAMPLE_TOPIC) self.take_sample_proxy = rospy.ServiceProxy(hec.TAKE_SAMPLE_TOPIC, hec.srv.TakeSample) rospy.wait_for_service(hec.REMOVE_SAMPLE_TOPIC) self.remove_sample_proxy = rospy.ServiceProxy(hec.REMOVE_SAMPLE_TOPIC, hec.srv.RemoveSample) rospy.wait_for_service(hec.COMPUTE_CALIBRATION_TOPIC) self.compute_calibration_proxy = rospy.ServiceProxy(hec.COMPUTE_CALIBRATION_TOPIC, hec.srv.ComputeCalibration) rospy.wait_for_service(hec.SAVE_CALIBRATION_TOPIC) self.save_calibration_proxy = rospy.ServiceProxy(hec.SAVE_CALIBRATION_TOPIC, std_srvs.srv.Empty)
def robot_listener(self): ''' rospy.wait_for_service('spawn') spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) spawner(4, 2, 0, 'turtle2') ''' robot_vel_pub = rospy.Publisher('robot_0/cmd_vel', geometry_msgs.msg.Twist, queue_size=1) rate = rospy.Rate(10.0) while not rospy.is_shutdown(): try: (trans,rot) = self.listener.lookupTransform('/robot_3', '/robot_0', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue angular = 4 * math.atan2(trans[1], trans[0]) linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2) cmd = geometry_msgs.msg.Twist() cmd.linear.x = linear cmd.angular.z = angular robot_vel_pub.publish(cmd) rate.sleep()
def __init__(self): rospy.init_node('gaze', anonymous=False) self.lock = threading.RLock() with self.lock: self.current_state = GazeState.IDLE self.last_state = self.current_state # Initialize Variables self.glance_timeout = 0 self.glance_timecount = 0 self.glance_played = False self.idle_timeout = 0 self.idle_timecount = 0 self.idle_played = False rospy.loginfo('\033[92m[%s]\033[0m waiting for bringup social_mind...'%rospy.get_name()) rospy.wait_for_service('environmental_memory/read_data') rospy.wait_for_service('social_events_memory/read_data') self.rd_memory = {} self.rd_memory['environmental_memory'] = rospy.ServiceProxy('environmental_memory/read_data', ReadData) self.rd_memory['social_events_memory'] = rospy.ServiceProxy('social_events_memory/read_data', ReadData) rospy.Subscriber('raising_events', RaisingEvents, self.handle_raising_events) rospy.Subscriber('gaze_focusing', String, self.handle_gaze_focusing) self.pub_gaze_cmd = rospy.Publisher('gaze_command', GazeCommand, queue_size=10) self.pub_viz_gaze_cmd = rospy.Publisher('visualization_gaze_cmd', PointStamped, queue_size=10) rospy.Timer(rospy.Duration(GAZE_CONTROLLER_PERIOD), self.handle_gaze_controller) rospy.loginfo('\033[92m[%s]\033[0m initialized...'%rospy.get_name()) rospy.spin()
def spawn_model(model_name): """ Spawns a model in front of the RGBD camera. Args: None Returns: None """ initial_pose = Pose() initial_pose.position.x = 0 initial_pose.position.y = 1 initial_pose.position.z = 1 # Spawn the new model # model_path = rospkg.RosPack().get_path('sensor_stick')+'/models/' model_xml = '' with open (model_path + model_name + '/model.sdf', 'r') as xml_file: model_xml = xml_file.read().replace('\n', '') spawn_model_prox = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) spawn_model_prox('training_model', model_xml, '', initial_pose, 'world')
def __init__(self, id, initialPosition, tf): self.id = id prefix = "/cf" + str(id) self.initialPosition = np.array(initialPosition) rospy.wait_for_service(prefix + "/upload_trajectory"); self.uploadTrajectoryService = rospy.ServiceProxy(prefix + "/upload_trajectory", UploadTrajectory) rospy.wait_for_service(prefix + "/set_ellipse"); self.setEllipseService = rospy.ServiceProxy(prefix + "/set_ellipse", SetEllipse) rospy.wait_for_service(prefix + "/takeoff") self.takeoffService = rospy.ServiceProxy(prefix + "/takeoff", Takeoff) rospy.wait_for_service(prefix + "/land") self.landService = rospy.ServiceProxy(prefix + "/land", Land) rospy.wait_for_service(prefix + "/hover") self.hoverService = rospy.ServiceProxy(prefix + "/hover", Hover) rospy.wait_for_service(prefix + "/avoid_target") self.avoidTargetService = rospy.ServiceProxy(prefix + "/avoid_target", AvoidTarget) rospy.wait_for_service(prefix + "/set_group") self.setGroupService = rospy.ServiceProxy(prefix + "/set_group", SetGroup) rospy.wait_for_service(prefix + "/update_params") self.updateParamsService = rospy.ServiceProxy(prefix + "/update_params", UpdateParams) self.tf = tf self.prefix = prefix
def __init__(self): # ROS initialization: rospy.init_node('pose_manager') self.poseLibrary = dict() self.readInPoses() self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction, execute_cb=self.executeBodyPose, auto_start=False) self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction) if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)): try: rospy.wait_for_service("stop_walk_srv", timeout=2.0) self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty) except: rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. " +"This is normal if there is no nao_walker running.") self.stopWalkSrv = None self.poseServer.start() rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys()); else: rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?"); rospy.signal_shutdown("Required component missing");
def test_timeout(self): entered = MagicMock() exited = MagicMock() self.my_task.steps['timed'].entered_handler = entered self.my_task.steps['timed'].exited_handler = exited rospy.wait_for_service("mytask_step") proxy = rospy.ServiceProxy( "mytask_step", StepTask ) response = proxy(name='timed') self.assertEqual(self.my_task.current_step.name, 'timed') entered.assert_called_once() rospy.sleep(0.2) self.assertEqual(self.my_task.current_step.name, 'say_hello') exited.assert_called_once() # Test that the timer does in fact reset rospy.sleep(0.2) self.assertEqual(self.my_task.current_step.name, 'say_hello')
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 ik_quaternion(self, quaternion_pose): node = ("ExternalTools/{}/PositionKinematicsNode/" "IKService".format(self.limb_name)) ik_service = rospy.ServiceProxy(node, SolvePositionIK) ik_request = SolvePositionIKRequest() ik_request.pose_stamp.append(quaternion_pose) try: rospy.loginfo('ik: waiting for IK service...') rospy.wait_for_service(node, 5.0) ik_response = ik_service(ik_request) except (rospy.ServiceException, rospy.ROSException) as err: rospy.logerr("ik_move: service request failed: %r" % err) else: if ik_response.isValid[0]: rospy.loginfo("ik_move: valid joint configuration found") limb_joints = dict(zip(ik_response.joints[0].name, ik_response.joints[0].position)) return limb_joints else: rospy.logerr('ik_move: no valid configuration found') return None
def __init__(self): self.services = {'record': {'name': 'perception/record', 'type': Record}} for service_name, service in self.services.items(): rospy.loginfo("Controller is waiting service {}...".format(service['name'])) rospy.wait_for_service(service['name']) service['call'] = rospy.ServiceProxy(service['name'], service['type']) # Buttons switches + LEDs self.prefix = 'sensors' self.button_leds_topics = ['button_leds/help', 'button_leds/pause'] self.buttons_topics = ['buttons/help', 'buttons/pause'] self.subscribers = [rospy.Subscriber('/'.join([self.prefix, topic]), Bool, lambda msg: self._cb_button_pressed(msg, topic)) for topic in self.buttons_topics] self.publishers = {topic: rospy.Publisher('/'.join([self.prefix, topic]), Bool, queue_size=1) for topic in self.button_leds_topics} self.button_leds_status = {topic: False for topic in self.button_leds_topics} self.button_pressed = {topic: False for topic in self.buttons_topics} self.last_press = {topic: rospy.Time(0) for topic in self.buttons_topics} self.rospack = RosPack() with open(join(self.rospack.get_path('apex_playground'), 'config', 'perception.json')) as f: self.params = json.load(f)
def __init__(self): self.services = {'set_iteration': {'name': 'learning/set_iteration', 'type': SetIteration}, 'set_focus': {'name': 'learning/set_interest', 'type': SetFocus}, 'assess': {'name': 'controller/assess', 'type': Assess}, 'get_interests': {'name': 'learning/get_interests', 'type': GetInterests}} rospy.Subscriber('learning/current_focus', String, self._cb_focus) rospy.Subscriber('learning/user_focus', String, self._cb_user_focus) rospy.Subscriber('learning/ready_for_interaction', Bool, self._cb_ready) self.current_focus = "" self.user_focus = "" self.ready_for_interaction = False for service_name, service in self.services.items(): rospy.loginfo("User node is waiting service {}...".format(service['name'])) rospy.wait_for_service(service['name']) service['call'] = rospy.ServiceProxy(service['name'], service['type']) rospy.loginfo("User node started!")
def _arm(self): print(self.namespace, 'arming') service_name = '%s/mavros/cmd/arming' % self.namespace rospy.wait_for_service(service_name) try: service = rospy.ServiceProxy(service_name, CommandBool) resp = service(True) except rospy.ServiceException as e: print(self.namespace, 'service call to arm failed:', str(e), file=sys.stderr) return False if not resp.success: print(self.namespace, 'failed to arm', file=sys.stderr) return False print(self.namespace, 'armed') return True
def _return_home(self): print(self.namespace, 'land') req = CommandTOLRequest() req.min_pitch = 0.0 req.yaw = -math.pi if self.color == 'blue' else 0.0 req.latitude = self.start_position.latitude req.longitude = self.start_position.longitude req.altitude = self.start_position.altitude service_name = '%s/mavros/cmd/land' % self.namespace rospy.wait_for_service(service_name) try: service = rospy.ServiceProxy(service_name, CommandTOL) resp = service.call(req) except rospy.ServiceException as e: print(self.namespace, 'service call to land failed:', str(e), file=sys.stderr) return False if not resp.success: print(self.namespace, 'failed to land', file=sys.stderr) return False print(self.namespace, 'landing') self._set_state(STATE_LANDING) return True
def delete_model(mav_sys_id, vehicle_type, ros_master_uri=None): if ros_master_uri: original_uri = os.environ[ROS_MASTER_URI] os.environ[ROS_MASTER_URI] = ros_master_uri srv = ServiceProxy('/gazebo/delete_model', DeleteModel) req = DeleteModelRequest() unique_name = vehicle_type + '_' + str(mav_sys_id) req.model_name = unique_name resp = srv(req) if ros_master_uri: os.environ[ROS_MASTER_URI] = original_uri if resp.success: print(resp.status_message, '(%s)' % unique_name) return 0 else: print("failed to delete model [%s]: %s" % (unique_name, resp.status_message), file=sys.stderr) return 1
def compute_fk_client(self, group, joint_values, links): rospy.wait_for_service('compute_fk') try: compute_fk = rospy.ServiceProxy('compute_fk', GetPositionFK) header = Header() header.stamp = rospy.Time.now() header.frame_id = group.get_pose_reference_frame() rs = RobotState() rs.joint_state.header = header rs.joint_state.name = group.get_active_joints() rs.joint_state.position = joint_values res = compute_fk(header, links, rs) return res.pose_stamped except rospy.ServiceException, e: print "Service call failed: %s" % e
def __init__(self, name): """ Constructor. @param name: camera identifier. You can get a list of valid identifiers by calling the ROS service /cameras/list. Expected names are right_hand_camera, left_hand_camera and head_camera. However if the cameras are not identified via the parameter server, they are simply indexed starting at 0. """ self._id = name self._open_svc = rospy.ServiceProxy('/cameras/open', OpenCamera) self._close_svc = rospy.ServiceProxy('/cameras/close', CloseCamera) self._settings = CameraSettings() self._settings.width = 320 self._settings.height = 200 self._settings.fps = 20 self._open = False
def __init__(self, topic_prefix, config_file, robot_config_file, write_lock, bag_file): self.topic_prefix = topic_prefix self.test_config = config_file self.robot_config_file = robot_config_file resources_timer_frequency = 100.0 # Hz self.timer_interval = 1/resources_timer_frequency self.res_pipeline = {} self.BfW = BagfileWriter(bag_file, write_lock) rospy.loginfo("Waiting for obstacle_distance node...") rospy.wait_for_service(self.robot_config_file["obstacle_distance"]["services"]) self.obstacle_distance_server = rospy.ServiceProxy(self.robot_config_file["obstacle_distance"]["services"], GetObstacleDistance) rospy.Timer(rospy.Duration.from_sec(self.timer_interval), self.collect_obstacle_distances)
def __init__(self, server): self.server = server self.mutex = Lock() # Publisher to send commands self.pub_command = rospy.Publisher("/motion_planning_goal", geometry_msgs.msg.Transform, queue_size=1) self.listener = tf.TransformListener() # Subscribes to information about what the current joint values are. rospy.Subscriber("/joint_states", sensor_msgs.msg.JointState, self.joint_states_callback) # Publisher to set robot position self.pub_reset = rospy.Publisher("/joint_command", JointState, queue_size=1) rospy.sleep(0.5) # Wait for validity check service rospy.wait_for_service("check_state_validity") self.state_valid_service = rospy.ServiceProxy('check_state_validity', moveit_msgs.srv.GetStateValidity) self.reset_robot()
def main(): trans= 0 rot = 0 rospy.init_node('odom_sync') listener = tf.TransformListener() pubfiltered = rospy.Publisher('set_pose',PoseWithCovarianceStamped,queue_size = 10) pubodom = rospy.Publisher('mobile_base/commands/reset_odometry', Empty, queue_size = 10) serv = rospy.ServiceProxy("set_offset",SetOdomOffset) rospy.Subscriber('/command_input',UInt8,callback,callback_args=(pubodom,pubfiltered,listener,trans,rot,serv)) rospy.sleep(1) print "done sleeping" while not rospy.is_shutdown(): try: (trans,rot) = listener.lookupTransform('/base_meas', '/odom_meas',rospy.Time(0)) except: continue rospy.spin()
def publish_to_ros(self, mode='transform', service_name='rigid_transforms/rigid_transform_publisher', namespace=None): """Publishes RigidTransform to ROS If a transform referencing the same frames already exists in the ROS publisher, it is updated instead. This checking is not order sensitive Requires ROS rigid_transform_publisher service to be running. Assuming autolab_core is installed as a catkin package, this can be done with: roslaunch autolab_core rigid_transforms.launch Parameters ---------- mode : :obj:`str` Mode in which to publish. In {'transform', 'frame'} Defaults to 'transform' service_name : string, optional RigidTransformPublisher service to interface with. If the RigidTransformPublisher services are started through rigid_transforms.launch it will be called rigid_transform_publisher namespace : string, optional Namespace to prepend to transform_listener_service. If None, current namespace is prepended. Raises ------ rospy.ServiceException If service call to rigid_transform_publisher fails """ if namespace == None: service_name = rospy.get_namespace() + service_name else: service_name = namespace + service_name rospy.wait_for_service(service_name, timeout = 10) publisher = rospy.ServiceProxy(service_name, RigidTransformPublisher) trans = self.translation rot = self.quaternion publisher(trans[0], trans[1], trans[2], rot[0], rot[1], rot[2], rot[3], self.from_frame, self.to_frame, mode)
def delete_from_ros(self, service_name='rigid_transforms/rigid_transform_publisher', namespace=None): """Removes RigidTransform referencing from_frame and to_frame from ROS publisher. Note that this may not be this exact transform, but may that references the same frames (order doesn't matter) Also, note that it may take quite a while for the transform to disappear from rigid_transform_publisher's cache Requires ROS rigid_transform_publisher service to be running. Assuming autolab_core is installed as a catkin package, this can be done with: roslaunch autolab_core rigid_transforms.launch Parameters ---------- service_name : string, optional RigidTransformPublisher service to interface with. If the RigidTransformPublisher services are started through rigid_transforms.launch it will be called rigid_transform_publisher namespace : string, optional Namespace to prepend to transform_listener_service. If None, current namespace is prepended. Raises ------ rospy.ServiceException If service call to rigid_transform_publisher fails """ if namespace == None: service_name = rospy.get_namespace() + service_name else: service_name = namespace + service_name rospy.wait_for_service(service_name, timeout = 10) publisher = rospy.ServiceProxy(service_name, RigidTransformPublisher) publisher(0, 0, 0, 0, 0, 0, 0, self.from_frame, self.to_frame, 'delete')
def rigid_transform_from_ros(from_frame, to_frame, service_name='rigid_transforms/rigid_transform_listener', namespace=None): """Gets transform from ROS as a rigid transform Requires ROS rigid_transform_publisher service to be running. Assuming autolab_core is installed as a catkin package, this can be done with: roslaunch autolab_core rigid_transforms.launch Parameters ---------- from_frame : :obj:`str` to_frame : :obj:`str` service_name : string, optional RigidTransformListener service to interface with. If the RigidTransformListener services are started through rigid_transforms.launch it will be called rigid_transform_listener namespace : string, optional Namespace to prepend to transform_listener_service. If None, current namespace is prepended. Raises ------ rospy.ServiceException If service call to rigid_transform_listener fails """ if namespace == None: service_name = rospy.get_namespace() + service_name else: service_name = namespace + service_name rospy.wait_for_service(service_name, timeout = 10) listener = rospy.ServiceProxy(service_name, RigidTransformListener) ret = listener(from_frame, to_frame) quat = np.asarray([ret.w_rot, ret.x_rot, ret.y_rot, ret.z_rot]) trans = np.asarray([ret.x_trans, ret.y_trans, ret.z_trans]) rot = RigidTransform.rotation_from_quaternion(quat) return RigidTransform(rotation=rot, translation=trans, from_frame=from_frame, to_frame=to_frame)
def fk_service_client(limb = "right"): ns = "ExternalTools/" + limb + "/PositionKinematicsNode/FKService" fksvc = rospy.ServiceProxy(ns, SolvePositionFK) fkreq = SolvePositionFKRequest() joints = JointState() joints.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6'] joints.position = [0.763331, 0.415979, -1.728629, 1.482985, -1.135621, -1.674347, -0.496337] # Add desired pose for forward kinematics fkreq.configuration.append(joints) # Request forward kinematics from base to "right_hand" link fkreq.tip_names.append('right_hand') try: rospy.wait_for_service(ns, 5.0) resp = fksvc(fkreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) return False # Check if result valid if (resp.isValid[0]): rospy.loginfo("SUCCESS - Valid Cartesian Solution Found") rospy.loginfo("\nFK Cartesian Solution:\n") rospy.loginfo("------------------") rospy.loginfo("Response Message:\n%s", resp) else: rospy.logerr("INVALID JOINTS - No Cartesian Solution Found.") return False return True
def __init__(self): self.force_reset = True self.vel_pub = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=5) self.unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) self.pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty) self.reset_proxy = rospy.ServiceProxy('/gazebo/reset_simulation', Empty) self.img_rows = 32 self.img_cols = 32 self.img_channels = 1
def __init__(self, robot_id): srv_str = "/robot%s/replace" % robot_id rospy.wait_for_service(srv_str) self.move = rospy.ServiceProxy(srv_str, MoveRobot) # TODO: handle rospy.service.ServiceException which can thrown from this self.starting_random_positions = None self.straight_section_poses = None # reset bot to a random position
def _create_service_client(self, srv_name): """ Create a service client proxy :param srv_name: Name of the service """ if self._srv: self._srv.close() if srv_name in rosservice.get_service_list(): rospy.loginfo("Creating proxy for service '%s'" % srv_name) self._srv = rospy.ServiceProxy(srv_name, rosservice.get_service_class_by_name(srv_name))
def _create_service_client(self, srv_name): """ Method that creates a client service proxy to call either the GetFaceProperties.srv or the Recognize.srv :param srv_name: """ if self._srv: self._srv.close() if srv_name in rosservice.get_service_list(): rospy.loginfo("Creating proxy for service '%s'" % srv_name) self._srv = rospy.ServiceProxy(srv_name, rosservice.get_service_class_by_name(srv_name))
def __init__(self, node_name): rospy.init_node(node_name, anonymous=False) try: conf_file = rospy.get_param('~config_file') except KeyError as e: rospy.logerr('You should set the parameter for perception config file...') quit() with open(os.path.abspath(conf_file)) as f: self.conf_data = yaml.load(f.read()) rospy.loginfo('loading perception config file... %d perception(s) exists...'%len(self.conf_data.keys())) for item in self.conf_data.keys(): rospy.loginfo('\033[92m - %s: %d event(s) and %d data(s).\033[0m'%(item, len(self.conf_data[item]['events']), len(self.conf_data[item]['data']))) self.dict_srv_wr = {} self.dict_srv_rd = {} for item in self.conf_data.keys(): if self.conf_data[item].has_key('target_memory'): memory_name = self.conf_data[item]['target_memory'] rospy.loginfo('\033[94m[%s]\033[0m wait for bringup %s...'%(rospy.get_name(), memory_name)) rospy.wait_for_service('/%s/write_data'%memory_name) self.dict_srv_wr[memory_name] = rospy.ServiceProxy('/%s/write_data'%memory_name, WriteData) self.dict_srv_rd[memory_name] = rospy.ServiceProxy('/%s/read_data'%memory_name, ReadData) self.register_data_to_memory(memory_name, item, self.conf_data[item]['data']) self.is_enable_perception = True rospy.Subscriber('%s/start'%rospy.get_name(), Empty, self.handle_start_perception) rospy.Subscriber('%s/stop'%rospy.get_name(), Empty, self.handle_stop_perception) self.pub_event = rospy.Publisher('forwarding_event', ForwardingEvent, queue_size=10) rospy.loginfo('\033[94m[%s]\033[0m initialize perception_base done...'%rospy.get_name())
def register_data_to_memory(self, memory_name, perception_name, data): rospy.wait_for_service('/%s/register_data'%memory_name) srv_register = rospy.ServiceProxy('/%s/register_data'%memory_name, RegisterData) srv_req = RegisterDataRequest() srv_req.perception_name = perception_name srv_req.data = json.dumps(data) return srv_register(srv_req)
def __getattr__(self, name): """ Override the __getattr__ method so that function calls become server requests If the name is a method of the YuMiArm class, this returns a function that calls that function on the YuMiArm instance in the server. The wait_for_res argument is not available remotely and will always be set to True. This is to prevent odd desynchronized crashes Otherwise, the name is considered to be an attribute, and getattr is called on the YuMiArm instance in the server. Note that if it isn't an attribute either a RuntimeError will be raised. The difference here is that functions access the server *on call* and non-functions do *on getting the name* Also note that this is __getattr__, so things like __init__ and __dict__ WILL NOT trigger this function as the YuMiArm_ROS object already has these as attributes. """ if name in YuMiArm.__dict__: def handle_remote_call(*args, **kwargs): """ Handle the remote call to some YuMiArm function. """ rospy.wait_for_service(self.arm_service, timeout = self.timeout) arm = rospy.ServiceProxy(self.arm_service, ROSYumiArm) if 'wait_for_res' in kwargs: kwargs['wait_for_res'] = True try: response = arm(pickle.dumps(name), pickle.dumps(args), pickle.dumps(kwargs)) except rospy.ServiceException, e: raise RuntimeError("Service call failed: {0}".format(str(e))) return pickle.loads(response.ret) return handle_remote_call else: rospy.wait_for_service(self.arm_service, timeout = self.timeout) arm = rospy.ServiceProxy(self.arm_service, ROSYumiArm) try: response = arm(pickle.dumps('__getattribute__'), pickle.dumps(name), pickle.dumps(None)) except rospy.ServiceException, e: raise RuntimeError("Could not get attribute: {0}".format(str(e))) return pickle.loads(response.ret)
def get_normals(cloud): get_normals_prox = rospy.ServiceProxy('/feature_extractor/get_normals', GetNormals) return get_normals_prox(cloud).cluster
def capture_sample(): """ Captures a PointCloud2 using the sensor stick RGBD camera Args: None Returns: PointCloud2: a single point cloud from the RGBD camrea """ get_model_state_prox = rospy.ServiceProxy('gazebo/get_model_state',GetModelState) model_state = get_model_state_prox('training_model','world') set_model_state_prox = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) roll = random.uniform(0, 2*math.pi) pitch = random.uniform(0, 2*math.pi) yaw = random.uniform(0, 2*math.pi) quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw) model_state.pose.orientation.x = quaternion[0] model_state.pose.orientation.y = quaternion[1] model_state.pose.orientation.z = quaternion[2] model_state.pose.orientation.w = quaternion[3] sms_req = SetModelStateRequest() sms_req.model_state.pose = model_state.pose sms_req.model_state.twist = model_state.twist sms_req.model_state.model_name = 'training_model' sms_req.model_state.reference_frame = 'world' set_model_state_prox(sms_req) return rospy.wait_for_message('/sensor_stick/point_cloud', PointCloud2)
def initial_setup(): """ Prepares the Gazebo world for generating training data. In particular, this routine turns off gravity, so that the objects spawned in front of the RGBD camera will not fall. It also deletes the ground plane, so that the only depth points produce will correspond to the object of interest (eliminating the need for clustering and segmentation as part of the trianing process) Args: None Returns: None """ rospy.wait_for_service('gazebo/get_model_state') rospy.wait_for_service('gazebo/set_model_state') rospy.wait_for_service('gazebo/get_physics_properties') rospy.wait_for_service('gazebo/set_physics_properties') rospy.wait_for_service('gazebo/spawn_sdf_model') get_physics_properties_prox = rospy.ServiceProxy('gazebo/get_physics_properties', GetPhysicsProperties) physics_properties = get_physics_properties_prox() physics_properties.gravity.z = 0 set_physics_properties_prox = rospy.ServiceProxy('gazebo/set_physics_properties', SetPhysicsProperties) set_physics_properties_prox(physics_properties.time_step, physics_properties.max_update_rate, physics_properties.gravity, physics_properties.ode_config) delete_model_prox = rospy.ServiceProxy('gazebo/delete_model', DeleteModel) delete_model_prox('ground_plane')
def delete_model(): # Delete the old model if it's stil around delete_model_prox = rospy.ServiceProxy('gazebo/delete_model', DeleteModel) delete_model_prox('training_model')
def perform_service_call(service_name): """ POST /api/<version>/service/<service_name> POST to a service to change it somehow. service_name may be a path. For example, to start an environmental recipe in an environment, use the start_recipe service: POST /api/<version>/service/<environment_id>/start_recipe {"recipe_id": <id>} """ # Add leading slash to service_name. ROS qualifies all services with a # leading slash. service_name = '/' + service_name # Find the class that ROS autogenerates from srv files that is associated # with this service. try: ServiceClass = rosservice.get_service_class_by_name(service_name) except rosservice.ROSServiceException as e: return error(e) # If we made it this far, the service exists. # Wait for service to be ready before attempting to use it. try: rospy.wait_for_service(service_name, 1) except rospy.ROSException as e: raise socket.error() # Get JSON args if they exist. If they don't, treat as if empty array # was passed. json = request.get_json(silent=True) args = json if json else [] service_proxy = rospy.ServiceProxy(service_name, ServiceClass) try: # Unpack the list of args and pass to service_proxy function. res = service_proxy(*args) except (rospy.ServiceException, TypeError) as e: return error(e) status_code = 200 if getattr(res, "success", True) else 400 data = {k: getattr(res, k) for k in res.__slots__} return jsonify(data), status_code
def call_service(service_name, result_msg, result_value, *args): # Add leading slash to service_name. ROS qualifies all services with a # leading slash. # Find the class that ROS autogenerates from srv files that is associated # with this service. ServiceClass = rosservice.get_service_class_by_name(service_name) rospy.wait_for_service(service_name, 1) service_proxy = rospy.ServiceProxy(service_name, ServiceClass) res = service_proxy(*args) print(service_name) print("-----------######-------------") print(res) assert getattr(res, "success", result_value) #assert res
def __init__(self): rospy.wait_for_service("/next_phase") self.nextPhase = rospy.ServiceProxy("/next_phase", Empty)
def __init__(self): rospy.init_node("CrazyflieAPI", anonymous=False) rospy.wait_for_service("/emergency") self.emergencyService = rospy.ServiceProxy("/emergency", Empty) rospy.wait_for_service("/takeoff") self.takeoffService = rospy.ServiceProxy("/takeoff", Takeoff) rospy.wait_for_service("/land") self.landService = rospy.ServiceProxy("/land", Land) rospy.wait_for_service("/start_trajectory"); self.startTrajectoryService = rospy.ServiceProxy("/start_trajectory", StartTrajectory) rospy.wait_for_service("/start_trajectory_reversed"); self.startTrajectoryReversedService = rospy.ServiceProxy("/start_trajectory_reversed", StartTrajectoryReversed) rospy.wait_for_service("/start_ellipse") self.ellipseService = rospy.ServiceProxy("/start_ellipse", StartEllipse) rospy.wait_for_service("/start_canned_trajectory") self.startCannedTrajectoryService = rospy.ServiceProxy("/start_canned_trajectory", StartCannedTrajectory) rospy.wait_for_service("/go_home"); self.goHomeService = rospy.ServiceProxy("/go_home", GoHome) rospy.wait_for_service("/update_params") self.updateParamsService = rospy.ServiceProxy("/update_params", UpdateParams) with open("../launch/crazyflies.yaml", 'r') as ymlfile: cfg = yaml.load(ymlfile) self.tf = TransformListener() self.crazyflies = [] self.crazyfliesById = dict() for crazyflie in cfg["crazyflies"]: id = int(crazyflie["id"]) initialPosition = crazyflie["initialPosition"] cf = Crazyflie(id, initialPosition, self.tf) self.crazyflies.append(cf) self.crazyfliesById[id] = cf
def init_ros(): global compute_com_area global compute_support_area rospy.init_node('real_time_control') rospy.wait_for_service( '/contact_stability/static/compute_support_area') rospy.wait_for_service( '/contact_stability/pendular/compute_support_area') compute_com_area = rospy.ServiceProxy( '/contact_stability/static/compute_support_area', contact_stability.srv.StaticArea) compute_support_area = rospy.ServiceProxy( '/contact_stability/pendular/compute_support_area', contact_stability.srv.PendularArea)
def _trigger_tobi(self, data): rospy.loginfo('trigger tobi') self._cam_sub = rospy.Subscriber( 'spqrel_pepper/camera/front/camera/image_raw', Image, self._image_cb ) self._depth_sub = rospy.Subscriber( 'spqrel_pepper/camera/depth/camera/image_raw', Image, self._depth_cb ) self._depth_info_sub = rospy.Subscriber( '/spqrel_pepper/camera/depth/camera/camera_info', CameraInfo, self._depth_info_cb ) # d = rospy.ServiceProxy('/ms_face_api/detect', Detect) # r = DetectRequest() # r.topic = 'spqrel_pepper/camera/front/camera/image_raw' # res = d.call(r) # res_faces = [] # for f in # res_dict = { # 'age': res.age, # 'gender': res.gender, # 'smile': res.smile # } # print json.dumps(res)
def test_services(self): ''' Test varius services that aren'ts tested individually ''' rospy.wait_for_service("mytask_step_name") proxy = rospy.ServiceProxy( "mytask_step_name", Message ) response = proxy(MessageRequest()) self.assertEqual(response.value, 'load') proxy = rospy.ServiceProxy( "mytask_task_payload", TaskPayload ) response = proxy() self.assertEqual(response.status, TaskStatusRequest.RUNNING) self.assertFalse(response.did_fail) proxy = rospy.ServiceProxy( "mytask_next_step", NextStep ) response = proxy(status=TaskStepStatus.SUCCESS) self.assertEqual(response.name, 'say_hello') response = proxy(status=TaskStepStatus.FAILURE) self.assertEqual(response.name, 'abort')
def test_status(self): rospy.wait_for_service("mytask_status") proxy = rospy.ServiceProxy( "mytask_status", TaskStatus ) response = proxy(TaskStatusRequest()) self.assertEqual(response.status, TaskStatusRequest.RUNNING)
def service_builder(self, service_def): """ Method for generating service endpoints for this Client. Each service contains a type and a service definition, as defined in the service's .srv file. For each service, a method is attached to the client that provides an endpoint with which other nodes can interact with the service. Because services must be uniquely namespaced, each service proxy will be exposed externally omitting the class's namespace as defined at construction time, but mapping to a service that uses the service class's namepspace. This means service proxies map as follows self.method('some message') -> /self.namespace_method Args: service (string): the name of the service """ service_name, service_type = service_def namespaced_service = '{}_{}'.format(self.namespace, service_name) def service_method(self, *args, **kwargs): rospy.wait_for_service(namespaced_service) service_proxy = rospy.ServiceProxy(namespaced_service, service_type) return service_proxy(self, *args, **kwargs) setattr(self, service_name, service_method)
def assign_task_proxies(self, task_name): """ Assigns service proxies for specified task. Args: task_name(str): Name of task to lookup services for. Will assign proxies by looking for service function with formatted string <task_name>_<service_name> """ for task_arr in self.proxy_services: service_name = "{}_{}".format(task_name, task_arr[0]) #loginfo("WAITING ON SERVIVCE {}".format(service_name)) rospy.wait_for_service(service_name) #loginfo("DONE WAITING ON SERVIVCE {}".format(service_name)) setattr( self.blackboard, "{}_proxy".format(task_arr[0]), rospy.ServiceProxy( service_name, task_arr[1] ) ) if self.task_step_serve_sub: self.task_step_serve_sub.unregister() # Subscribe to topics to failure or success of task steps self.task_step_serve_sub = rospy.Subscriber( '/needybot/{}/step/serve'.format(task_name), ros_msg.String, self.task_step_serve_handler ) # Call reset on task self.blackboard.reset_proxy(EmptyRequest())
def run(self): """ Run our code """ rospy.loginfo("Start laser test code") rospy.wait_for_service("assemble_scans2") # Todo - publish the spin logic... self.laser_publisher = rospy.Publisher("/multisense/lidar_cloud", PointCloud, queue_size=10) self.scan_service = rospy.ServiceProxy('assemble_scans', AssembleScans) #self.bridge = CvBridge() #self.zarj_os.zps.look_down() while True: begin = rospy.get_rostime() rospy.sleep(3.0) resp = self.scan_service(begin, rospy.get_rostime()) self.laser_publisher.publish(resp.cloud) #print "Got cloud {}x{}".format(resp.cloud.height, resp.cloud.width) img = self.create_image_from_cloud(resp.cloud.points) cv2.destroyAllWindows() print "New image" cv2.imshow("My image", img) cv2.waitKey(1) #print resp #cv_image = self.bridge.imgmsg_to_cv2(resp.cloud, "bgr8") #cv2.imshow("Point cloud", cv_image)
def start(self, name): """ Start the field computer """ rospy.init_node(name) self.oclog('Getting robot_name') while not rospy.has_param('/ihmc_ros/robot_name'): print "-------------------------------------------------" print "Cannot run team_zarj_main.py, missing parameters!" print "Missing parameter '/ihmc_ros/robot_name'" print "Likely connection to ROS not present. Retry in 1 second." print "-------------------------------------------------" time.sleep(1) self.oclog('Booting ZarjOS') self.zarj = ZarjOS() self.start_task_service = rospy.ServiceProxy( "/srcsim/finals/start_task", StartTask) self.task_subscriber = rospy.Subscriber( "/srcsim/finals/task", Task, self.task_status) self.harness_subscriber = rospy.Subscriber( "/srcsim/finals/harness", Harness, self.harness_status) if HAS_SCORE: self.score_subscriber = rospy.Subscriber( "/srcsim/finals/score", Score, self.score_status) rate = rospy.Rate(10) # 10hz if self.task_subscriber.get_num_connections() == 0: self.oclog('waiting for task publisher...') while self.task_subscriber.get_num_connections() == 0: rate.sleep() if self.harness_subscriber.get_num_connections() == 0: self.oclog('waiting for harness publisher...') while self.harness_subscriber.get_num_connections() == 0: rate.sleep() self.oclog('...got it, field computer is operational!')
def __init__(self): # self.cylinder_detector_client = rospy.ServiceProxy('cylinderdata',cylinder.cylinderdata) self.cylinder_opencv_client = rospy.ServiceProxy('cylinder_data',cylinder_opencv.cylinder_find) self.cylinder_laser_client = rospy.ServiceProxy('cylinder',cylinder_laser.cylinder) rospy.loginfo('[cylinder_state_pkg]->waiting cylinderdata service') self.cylinder_laser_client.wait_for_service() self.cylinder_opencv_client.wait_for_service() rospy.loginfo('[cylinder_state_pkg] -> connected to cylinder service') #?????????????????x?????????
