def __init__(self):
self.run_recognition = rospy.get_param('/rgb_object_detection/run_recognition')
self.model_filename = rospy.get_param('/rgb_object_detection/model_file')
self.weights_filename = rospy.get_param('/rgb_object_detection/weights_file')
self.categories_filename = rospy.get_param('/rgb_object_detection/category_file')
self.verbose = rospy.get_param('/rgb_object_detection/verbose', False)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber('/camera/rgb/image_color', Image, self.img_cb)
self.patches_sub = rospy.Subscriber('/candidate_regions_depth', PolygonStamped, self.patches_cb)
self.detection_pub = rospy.Publisher('/detections', Detection, queue_size=1)
# you can read this value off of your sensor from the '/camera/depth_registered/camera_info' topic
self.P = np.array([[525.0, 0.0, 319.5, 0.0], [0.0, 525.0, 239.5, 0.0], [0.0, 0.0, 1.0, 0.0]])
if self.run_recognition:
self.cnn = CNN('', self.model_filename, self.weights_filename, self.categories_filename, '', 0, 0, self.verbose)
self.cnn.load_model()
python类Subscriber()的实例源码
def _start(self, spin):
for args, kwargs in self.subscribers:
self.subscribers_init.append(rospy.Subscriber(*args, **kwargs))
is_func = isinstance(self.cl, types.FunctionType)
is_class = isinstance(self.cl, types.TypeType)
if is_class:
targ = self.__start_class
elif is_func:
targ = self.__start_func
self.thread = threading.Thread(target=targ,
args=(self.cl,) + self.cl_args,
kwargs=self.cl_kwargs)
self.thread.daemon = True
self.thread.start()
if spin:
rospy.spin()
return self
tf_broadcast_transform_pre_cleanup.py 文件源码
项目：amosero
作者: PaulPetring
项目源码
文件源码
阅读 16
收藏 0
点赞 0
评论 0
def listener():
global odom_pub,left_motor_pub,left_motor_speed_pub,right_motor_pub,right_motor_speed_pub
#subscribing to required topics
rospy.Subscriber("/cmd_vel", Twist, cmd_velCb)
rospy.Subscriber('imu_data',Imu,imuCb)
rospy.Subscriber('north',std_msgs.msg.Float32,northCb)
odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
left_motor_pub = rospy.Publisher('/left_motor', std_msgs.msg.Int32, queue_size=10)
right_motor_pub = rospy.Publisher('/right_motor', std_msgs.msg.Int32, queue_size=10)
#left_motor_speed_pub = rospy.Publisher('/left_motor_speed', std_msgs.msg.Int32, queue_size=10)
#right_motor_speed_pub = rospy.Publisher('/right_motor_speed', std_msgs.msg.Int32, queue_size=10)
rospy.spin()
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 listener():
global odom_pub,left_motor_pub,left_motor_speed_pub,right_motor_pub,right_motor_speed_pub
#subscribing to required topics
rospy.Subscriber("/cmd_vel", Twist, cmd_velCb)
rospy.Subscriber('imu_data',Imu,imuCb)
rospy.Subscriber('north',std_msgs.msg.Float32,northCb)
odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
left_motor_pub = rospy.Publisher('/left_motor', std_msgs.msg.Int32, queue_size=10)
right_motor_pub = rospy.Publisher('/right_motor', std_msgs.msg.Int32, queue_size=10)
#left_motor_speed_pub = rospy.Publisher('/left_motor_speed', std_msgs.msg.Int32, queue_size=10)
#right_motor_speed_pub = rospy.Publisher('/right_motor_speed', std_msgs.msg.Int32, queue_size=10)
rospy.spin()
def listener():
global odom_pub,left_motor_pub,left_motor_speed_pub,right_motor_pub,right_motor_speed_pub
#subscribing to required topics
rospy.Subscriber("/cmd_vel", Twist, cmd_velCb)
rospy.Subscriber('imu_data',Imu,imuCb)
rospy.Subscriber('north',std_msgs.msg.Float32,northCb)
odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
left_motor_pub = rospy.Publisher('/left_motor', std_msgs.msg.Int32, queue_size=10)
right_motor_pub = rospy.Publisher('/right_motor', std_msgs.msg.Int32, queue_size=10)
#left_motor_speed_pub = rospy.Publisher('/left_motor_speed', std_msgs.msg.Int32, queue_size=10)
#right_motor_speed_pub = rospy.Publisher('/right_motor_speed', std_msgs.msg.Int32, queue_size=10)
rospy.spin()
def test_publish_to_topics(self):
topic_ending = "desired"
rospy.logdebug("Sleeping for 5 seconds to let ROS spin up...")
rospy.sleep(5)
for variable, value in self.variables:
# Publish to each variable/desired topic to see if all of the
# actuators come on as expected.
topic_string = variable + "/" + topic_ending
rospy.logdebug("Testing Publishing to " + topic_string)
pub_desired = rospy.Publisher(topic_string,
Float64, queue_size=10)
sub_desired = rospy.Subscriber(topic_string, Float64, self.callback)
rospy.sleep(2)
print(self.namespace + "/" + topic_string)
for _ in range(NUM_TIMES_TO_PUBLISH):
pub_desired.publish(value)
rospy.sleep(1)
rospy.sleep(2)
pub_desired.publish(0)
def ros_loop(test):
while True:
rospy.Subscriber('human_turn_topic', String, human_turn)
rospy.Subscriber('human_chosen_topic', String, have_chosen)
rospy.Subscriber('human_predict_turn_topic', String, human_predict)
rospy.Subscriber('robot_turn_topic', String, robot_turn)
rospy.Subscriber('robot_chosen_topic', String, have_chosen)
rospy.Subscriber('story_telling', String, new_phrase)
rospy.Subscriber('new_element', String, new_element)
rospy.sleep(0.1)
rospy.spin()
################################################
def ros_loop(test):
while True:
rospy.Subscriber('human_turn_topic', String, human_turn)
rospy.Subscriber('human_chosen_topic', String, have_chosen)
rospy.Subscriber('human_predict_turn_topic', String, human_predict)
rospy.Subscriber('robot_turn_topic', String, robot_turn)
rospy.Subscriber('robot_chosen_topic', String, have_chosen)
rospy.Subscriber('story_telling', String, new_phrase)
rospy.Subscriber('new_element', String, new_element)
rospy.sleep(0.1)
rospy.spin()
################################################
def ros_loop(test):
while True:
rospy.Subscriber('human_turn_topic', String, human_turn)
rospy.Subscriber('human_chosen_topic', String, have_chosen)
rospy.Subscriber('human_predict_turn_topic', String, human_predict)
rospy.Subscriber('robot_turn_topic', String, robot_turn)
rospy.Subscriber('robot_chosen_topic', String, have_chosen)
rospy.Subscriber('story_telling', String, new_phrase)
rospy.Subscriber('new_element', String, new_element)
rospy.sleep(0.1)
rospy.spin()
################################################
def main():
rospy.init_node("actions")
loginfo("Creating action handler...")
action_handler = ActionHandler()
loginfo("Registering services...")
get_service_handler(CallFunction).register_service(
lambda func_name, kwargs: action_handler.call_func(func_name, **json.loads(kwargs))
)
rospy.Subscriber("/aide/update_apis", String, lambda x: action_handler.reload_api_references(x.data))
get_service_handler(GetActionProvider).register_service(lambda name: action_handler.get_action_provider(name) or ())
get_service_handler(GetAllActionProviders).register_service(action_handler.get_all_action_providers)
get_service_handler(AddActionProvider).register_service(action_handler.add_action_provider)
get_service_handler(DeleteActionProvider).register_service(action_handler.remove_action_provider)
loginfo("Registered services. Spinning.")
rospy.spin()
def __init__(self):
self._read_configuration()
if self.show_plots:
self._setup_plots()
rospy.loginfo("Receiving timestamp messages from {}".format(self.uwb_timestamps_topic))
rospy.loginfo("Publishing multi-range messages to {}".format(self.uwb_multi_range_topic))
rospy.loginfo("Publishing raw multi-range messages to {}".format(self.uwb_multi_range_raw_topic))
rospy.loginfo("Publishing multi-range-with-offsets messages to {}".format(
self.uwb_multi_range_with_offsets_topic))
# ROS Publishers
self.uwb_pub = rospy.Publisher(self.uwb_multi_range_topic, uwb.msg.UWBMultiRange, queue_size=1)
self.uwb_raw_pub = rospy.Publisher(self.uwb_multi_range_raw_topic, uwb.msg.UWBMultiRange, queue_size=1)
self.uwb_with_offsets_pub = rospy.Publisher(self.uwb_multi_range_with_offsets_topic,
uwb.msg.UWBMultiRangeWithOffsets, queue_size=1)
self.uwb_timestamps_sub = rospy.Subscriber(self.uwb_timestamps_topic, uwb.msg.UWBMultiRangeTimestamps,
self.handle_timestamps_message)
# Variables for rate display
self.msg_count = 0
self.last_now = rospy.get_time()
def __init__(self):
"""Initialize tracker.
"""
self._read_configuration()
self.estimates = {}
self.estimate_times = {}
self.ikf_prev_outlier_flags = {}
self.ikf_outlier_counts = {}
self.outlier_thresholds = {}
rospy.loginfo("Receiving raw multi-range messages from: {}".format(self.uwb_multi_range_topic))
rospy.loginfo("Publishing tracker messages to {}".format(self.uwb_tracker_topic))
rospy.loginfo("Publishing tracker transform as {} -> {}".format(self.tracker_frame, self.target_frame))
# ROS publishers and subscribers
self.tracker_frame = self.tracker_frame
self.target_frame = self.target_frame
self.uwb_pub = rospy.Publisher(self.uwb_tracker_topic, uwb.msg.UWBTracker, queue_size=1)
self.tf_broadcaster = tf.TransformBroadcaster()
self.uwb_multi_range_sub = rospy.Subscriber(self.uwb_multi_range_topic, uwb.msg.UWBMultiRangeWithOffsets,
self.handle_multi_range_message)
def run(self):
self.topic_type = wait_topic_ready(self.topic_name, self.url)
if not self.topic_type:
rospy.logerr('Type of topic %s are not equal in the remote and local sides', self.topic_name)
return
topic_type_module, topic_type_name = tuple(self.topic_type.split('/'))
try:
roslib.load_manifest(topic_type_module)
msg_module = import_module(topic_type_module + '.msg')
self.rostype = getattr(msg_module, topic_type_name)
self.subscriber = rospy.Subscriber(self.topic_name, self.rostype, self.callback)
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 for published topic %s successfully', self.topic_name)
self.ws.run_forever()
except ResourceNotFound, e:
rospy.logerr('Could not find the required resource %s', str(e))
except Exception, e:
rospy.logerr('Proxy for published topic %s init falied. Reason: %s', self.topic_name, str(e))
def __init__(self):
if rospy.has_param('~orientation_offset'):
# Orientation offset as quaterion q = [x,y,z,w].
self.orientation_offset = rospy.get_param('~orientation_offset')
else:
yaw_offset_deg = rospy.get_param('~yaw_offset_deg', 0.0)
self.orientation_offset = tf.quaternion_from_euler(0.0, 0.0, math.radians(yaw_offset_deg))
rospy.Subscriber(rospy.get_name() + "/imu_in", Imu, self.imu_callback)
self.pub_imu_out = rospy.Publisher(rospy.get_name() + '/imu_out',
Imu, queue_size=10)
rospy.spin()
def __init__(self):
# Read Settings
self.read_settings()
# Init other variables
self._num_magnetometer_reads = 0
self._latest_bearings = np.zeros(shape = (self._number_samples_average, 1))
self._received_enough_samples = False
# Subscribe to magnetometer topic
rospy.Subscriber("magnetic_field", Vector3Stamped, self.magnetic_field_callback)
# Publishers
self._pub_bearing_raw = rospy.Publisher(rospy.get_name() + '/bearing_raw_deg',
Float64, queue_size = 10)
self._pub_bearing_avg = rospy.Publisher(rospy.get_name() + '/bearing_avg_deg',
Float64, queue_size = 10)
self._pub_imu_bearing_avg = rospy.Publisher(rospy.get_name() + '/imu_bearing_avg',
Imu, queue_size = 10)
if self._verbose:
self._pub_mag_corrected = rospy.Publisher(rospy.get_name() + '/mag_corrected',
Vector3Stamped, queue_size = 10)
rospy.spin()
def main():
rospy.init_node("listener")
sub = rospy.Subscriber("/chatter_topic", String, callback)
rospy.spin()
def subscribePose():
rospy.Subscriber('/initialpose', PoseWithCovarianceStamped, getPoseData)
# rospy.Subscriber('/move_base_simple/goal', PoseStamped, getPoseData)
global background
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 __init__(self):
"""
HomeJoints - Class that publishes on /robot/set_homing_mode to
home the robot if it is not already homed.
"""
self._hcb_lock = threading.Lock()
self._ecb_lock = threading.Lock()
self._homing_state = dict()
self._enable_state = False
self._pub_home_joints = rospy.Publisher(
'/robot/set_homing_mode',
HomingCommand,
latch=True,
queue_size=10)
self._enable_sub = rospy.Subscriber(
'/robot/state',
AssemblyState,
self._enable_state_cb)
self._homing_sub = rospy.Subscriber(
'/robot/homing_states',
HomingState,
self._homing_state_cb)
def __init__(self):
"""
Constructor.
"""
self._state = dict()
self._pub_pan = rospy.Publisher(
'/robot/head/command_head_pan',
HeadPanCommand,
queue_size=10)
state_topic = '/robot/head/head_state'
self._sub_state = rospy.Subscriber(
state_topic,
HeadState,
self._on_head_state)
self._tf_listener = tf.TransformListener()
intera_dataflow.wait_for(
lambda: len(self._state) != 0,
timeout=5.0,
timeout_msg=("Failed to get current head state from %s" %
(state_topic,)),
)
def __init__(self, side="right", calibrate=True):
"""
Constructor.
@type side: str
@param side: robot gripper name
@type calibrate: bool
@param calibrate: Attempts to calibrate the gripper when initializing class (defaults True)
"""
self.devices = None
self.name = '_'.join([side, 'gripper'])
self.ee_config_sub = rospy.Subscriber('/io/end_effector/config', IONodeConfiguration, self._config_callback)
# Wait for the gripper device status to be true
intera_dataflow.wait_for(
lambda: not self.devices is None, timeout=5.0,
timeout_msg=("Failed to get gripper. No gripper attached on the robot.")
)
self.gripper_io = IODeviceInterface("end_effector", self.name)
if self.has_error():
self.reboot()
calibrate = True
if calibrate and not self.is_calibrated():
self.calibrate()
def start():
global left_motor_pub,right_motor_pub
# publishing to "turtle1/cmd_vel" to control turtle1
global pub
pub = rospy.Publisher('turtle1/cmd_vel', Twist)
left_motor_pub = rospy.Publisher('/left_motor', std_msgs.msg.Int32, queue_size=10)
right_motor_pub = rospy.Publisher('/right_motor', std_msgs.msg.Int32, queue_size=10)
# subscribed to joystick inputs on topic "joy"
rospy.Subscriber("joy", Joy, callback)
# starts the node
rospy.init_node('Joy2Turtle')
rospy.spin()
def listener():
global odom_pub,left_motor_pub,left_motor_speed_pub,right_motor_pub,right_motor_speed_pub
rospy.Subscriber("/cmd_vel", Twist, callback)
rospy.Subscriber('imu_data',Imu,imuCb)
rospy.Subscriber('north',std_msgs.msg.Float32,northCb)
odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
left_motor_pub = rospy.Publisher('/left_motor', std_msgs.msg.Int32, queue_size=10)
left_motor_speed_pub = rospy.Publisher('/left_motor_speed', std_msgs.msg.Int32, queue_size=10)
right_motor_pub = rospy.Publisher('/right_motor', std_msgs.msg.Int32, queue_size=10)
right_motor_speed_pub = rospy.Publisher('/right_motor_speed', std_msgs.msg.Int32, queue_size=10)
rospy.spin()
def listener():
global odom_pub,left_motor_pub,left_motor_speed_pub,right_motor_pub,right_motor_speed_pub
rospy.Subscriber("/cmd_vel", Twist, callback)
odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
left_motor_pub = rospy.Publisher('/left_motor', std_msgs.msg.Int32, queue_size=10)
left_motor_speed_pub = rospy.Publisher('/left_motor_speed', std_msgs.msg.Int32, queue_size=10)
right_motor_pub = rospy.Publisher('/right_motor', std_msgs.msg.Int32, queue_size=10)
right_motor_speed_pub = rospy.Publisher('/right_motor_speed', std_msgs.msg.Int32, queue_size=10)
rospy.spin()
def start():
global left_motor_pub,right_motor_pub
# publishing to "turtle1/cmd_vel" to control turtle1
global pub
pub = rospy.Publisher('turtle1/cmd_vel', Twist)
left_motor_pub = rospy.Publisher('/left_motor', std_msgs.msg.Int32, queue_size=10)
right_motor_pub = rospy.Publisher('/right_motor', std_msgs.msg.Int32, queue_size=10)
# subscribed to joystick inputs on topic "joy"
rospy.Subscriber("joy", Joy, callback)
# starts the node
rospy.init_node('Joy2Turtle')
rospy.spin()
def start():
global left_motor_pub,right_motor_pub
# publishing to "turtle1/cmd_vel" to control turtle1
global pub
pub = rospy.Publisher('turtle1/cmd_vel', Twist)
left_motor_pub = rospy.Publisher('/left_motor', std_msgs.msg.Int32, queue_size=10)
right_motor_pub = rospy.Publisher('/right_motor', std_msgs.msg.Int32, queue_size=10)
# subscribed to joystick inputs on topic "joy"
rospy.Subscriber("joy", Joy, callback)
# starts the node
rospy.init_node('Joy2Turtle')
rospy.spin()
def start():
global left_motor_pub,right_motor_pub
# publishing to "turtle1/cmd_vel" to control turtle1
global pub
pub = rospy.Publisher('turtle1/cmd_vel', Twist)
left_motor_pub = rospy.Publisher('/left_motor', std_msgs.msg.Int32, queue_size=10)
right_motor_pub = rospy.Publisher('/right_motor', std_msgs.msg.Int32, queue_size=10)
# subscribed to joystick inputs on topic "joy"
rospy.Subscriber("joy", Joy, callback)
# starts the node
rospy.init_node('Joy2Turtle')
rospy.spin()
def __init__(self):
# initialize a socket
self.host = rospy.get_param('~server_address', '127.0.0.1')
self.port = rospy.get_param('~server_port', 20011)
self.friend_color = rospy.get_param('/friend_color', 'yellow')
rospy.loginfo('server address is set to [' + self.host + ']')
rospy.loginfo('server port is set to [' + str(self.port) + ']')
rospy.loginfo('team color is set to [' + self.friend_color + ']')
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# make subscribers
self.subscribers = []
for i in xrange(12):
topic = "/robot_" + str(i) + "/robot_commands"
self.subscribers.append(
rospy.Subscriber(
topic,
robot_commands,
self.sendCommands,
callback_args=i))
avoid_runaway_suppressor.py 文件源码
项目：micros_mars_task_alloc
作者: liminglong
项目源码
文件源码
阅读 16
收藏 0
点赞 0
评论 0
def avoid_runaway_suppressor():
rospy.init_node('avoid_runaway_suppressor')
rospy.Subscriber("/avoid/suppressor/heading", Heading, avoid_headingCB)
rospy.Subscriber("/runaway/suppressor/heading", Heading, runaway_headingCB)
rospy.spin()
