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
python类Service()的实例源码
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
control_state.py 文件源码
项目：master_robot_strage
作者: nwpu-basketball-robot
项目源码
文件源码
阅读 18
收藏 0
点赞 0
评论 0
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))
