我们从Python开源项目中，提取了以下50个代码示例，用于说明如何使用rospy.wait_for_service()。
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): 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 save_to_memory(self, perception_name, data={}): if data == {}: rospy.logwarn('Empty data inserted...') return for item in data.keys(): if not item in self.conf_data[perception_name]['data'].keys(): rospy.logwarn('[%s -- wrong data inserted [%s]...'%(perception_name, item)) return srv_req = WriteDataRequest() srv_req.perception_name = perception_name srv_req.data = json.dumps(data) srv_req.by = rospy.get_name() target_memory = self.conf_data[perception_name]['target_memory'] try: rospy.wait_for_service('/%s/write_data'%target_memory) self.dict_srv_wr[target_memory](srv_req) except rospy.ServiceException as e: pass
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 __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 publish(topic, instructions=None): if type(topic) not in [str, nb_channels.Messages]: rospy.logerr("** Topic must be of type string or Messages in order to publish to cable **") return False if instructions and type(instructions) is not dict: rospy.logerr("** Instructions must be of type dictionary in order to publish to cable **") return False # Properly format topic string topic_val = topic if type(topic) is str else topic.value try: rospy.wait_for_service('ui_send', 0.1) client = UIClient() return client.send( topic_val, json.dumps(instructions) ) except rospy.ROSException: return UIMessageResponse(success=False)
def init_traj(self): try: # self.recorder.init_traj(itr) if self.use_goalimage: goal_img_main, goal_state = self.load_goalimage() goal_img_aux1 = np.zeros([64, 64, 3]) else: goal_img_main = np.zeros([64, 64, 3]) goal_img_aux1 = np.zeros([64, 64, 3]) goal_img_main = self.bridge.cv2_to_imgmsg(goal_img_main) goal_img_aux1 = self.bridge.cv2_to_imgmsg(goal_img_aux1) rospy.wait_for_service('init_traj_visualmpc', timeout=1) self.init_traj_visual_func(0, 0, goal_img_main, goal_img_aux1, self.save_subdir) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) raise ValueError('get_kinectdata service failed')
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 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 execute_iteration(self, task, method, iteration, trial, num_iterations): rospy.logwarn("Controller starts iteration {} {}/{} trial {}".format(method, iteration, num_iterations, trial)) rospy.wait_for_service('ergo/reset') # Ensures Ergo keeps working or wait till it reboots # After resuming, we keep the same iteration if self.perception.has_been_pressed('buttons/help'): rospy.sleep(1.5) # Wait for the robot to fully stop self.recorder.record(task, method, trial, iteration) self.perception.switch_led('button_leds/pause', True) recording = self.perception.record(human_demo=True, nb_points=self.params['nb_points']) self.torso.set_torque_max(self.torso_params['torques']['reset']) self.torso.reset(slow=True) return True else: trajectory = self.learning.produce(skill_to_demonstrate=self.demonstrate).torso_trajectory self.torso.set_torque_max(self.torso_params['torques']['motion']) self.recorder.record(task, method, trial, iteration) self.perception.switch_led('button_leds/pause', True) self.torso.execute_trajectory(trajectory) # TODO: blocking, non-blocking, action server? recording = self.perception.record(human_demo=False, nb_points=self.params['nb_points']) self.perception.switch_led('button_leds/pause', False) recording.demo.torso_demonstration = JointTrajectory() self.torso.set_torque_max(80) self.torso.reset(slow=False) return self.learning.perceive(recording.demo)
def __init__(self, worker_id, outside_ros=False): self.worker_id = worker_id self.outside_ros = outside_ros if self.outside_ros: rospy.logwarn('Controller is using ZMQ to get work') self.context = Context() self.socket = self.context.socket(REQ) self.socket.connect('tcp://127.0.0.1:33589') else: rospy.logwarn('Controller is using ROS to get work') self.services = {'get': {'name': '/work/get', 'type': GetWork}, 'update': {'name': '/work/update', 'type': UpdateWorkStatus}} 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'])
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 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 reset_robot(): """ Call to a Reset ROS service if available """ rospy.wait_for_service('reset_positions') return # ToDo: Implement the following functions to use an arm in ROS # def moveArm(v): # return # # def moveBiceps(v): # return # # def moveForearm(v): # return # # def stopArmAll(v): # return # # def getGripperPose3d(): # return np.array(pos) # # def getGoalPose3d(): # return np.array(pos)
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 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, 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 __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 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 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 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 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 get_service(self): """ Returns the service which then can be called. !! Does not call the service. :return: the service. """ rospy.wait_for_service(self.SERVICE_CHANNEL) return rospy.ServiceProxy(self.SERVICE_CHANNEL, self.service_class)
def call_service(self, **kwargs): """ Calls the service. :param kwargs: Arguments of the service. :return: Result of the service call. """ rospy.wait_for_service(self.SERVICE_CHANNEL) return rospy.ServiceProxy(self.SERVICE_CHANNEL, self.service_class)(**kwargs) # Create a list of all Services that are implemented
def robot_inference_client(x): rospy.wait_for_service('inference') try: infer = rospy.ServiceProxy('inference', Inference) resp1 = infer(x) return resp1.inference except rospy.ServiceException, e: print ("Service call failed: %s"%e)
def get_endeffector_pos(self, pos_only=True): """ :param pos_only: only return postion :return: """ fkreq = SolvePositionFKRequest() joints = JointState() joints.name = self.ctrl.limb.joint_names() joints.position = [self.ctrl.limb.joint_angle(j) for j in joints.name] # Add desired pose for forward kinematics fkreq.configuration.append(joints) fkreq.tip_names.append('right_hand') try: rospy.wait_for_service(self.name_of_service, 5) resp = self.fksvc(fkreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) return False pos = np.array([resp.pose_stamp[0].pose.position.x, resp.pose_stamp[0].pose.position.y, resp.pose_stamp[0].pose.position.z]) return pos
def query_action(self): if self.use_robot: if self.use_aux: self.recorder.get_aux_img() imageaux1 = self.recorder.ltob_aux1.img_msg else: imageaux1 = np.zeros((64, 64, 3), dtype=np.uint8) imageaux1 = self.bridge.cv2_to_imgmsg(imageaux1) imagemain = self.bridge.cv2_to_imgmsg(self.recorder.ltob.img_cropped) state = self.get_endeffector_pos() else: imagemain = np.zeros((64,64,3)) imagemain = self.bridge.cv2_to_imgmsg(imagemain) imageaux1 = self.bridge.cv2_to_imgmsg(self.test_img) state = np.zeros(3) try: rospy.wait_for_service('get_action', timeout=240) get_action_resp = self.get_action_func(imagemain, imageaux1, tuple(state), tuple(self.desig_pos_main.flatten()), tuple(self.goal_pos_main.flatten())) action_vec = get_action_resp.action except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) raise ValueError('get action service call failed') return action_vec
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.loginfo("INVALID JOINTS - No Cartesian Solution Found.") return True
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 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 ik_quaternion(self, quaternion_pose): """Take a xyz qxqyqzqw stamped pose and convert it to joint angles using IK. You can call self.limb.move_to_joint_positions to move to those angles """ 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") # convert response to joint position control dictionary 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
