我们从Python开源项目中，提取了以下50个代码示例，用于说明如何使用rospy.Subscriber()。
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()
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
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 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 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 __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))
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()
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): self.map = None self.start = None self.goal = None self.moves = [Move(0.1, 0), # forward Move(-0.1, 0), # back Move(0, 1.5708), # turn left 90 Move(0, -1.5708)] # turn right 90 self.robot = Robot(0.5, 0.5) self.is_working = False # Replace with mutex after all self.map_subscriber = rospy.Subscriber("map", OccupancyGrid, self.new_map_callback) self.start_subscriber = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.new_start_callback) self.goal_subscriber = rospy.Subscriber("goal", PoseStamped, self.new_goal_callback) self.path_publisher = rospy.Publisher("trajectory", MarkerArray, queue_size=1) self.pose_publisher = rospy.Publisher("debug_pose", PoseStamped, queue_size=1) # what will be there. A module goes into variable. Isn't it too much memory consumption. Maybe I should assign function replan() to this variable? self.planner = planners.astar.replan
def connect_topics( src_topic, dest_topic, src_topic_type, dest_topic_type, multiplier=1, deadband=0 ): rospy.loginfo("Connecting topic {} to topic {}".format( src_topic, dest_topic )) pub = rospy.Publisher(dest_topic, dest_topic_type, queue_size=10) def callback(src_item): val = src_item.data val *= multiplier if dest_topic_type == Bool: val = (val > deadband) dest_item = dest_topic_type(val) pub.publish(dest_item) sub = rospy.Subscriber(src_topic, src_topic_type, callback) return sub, pub
def stream_topic(topic_name): """ GET /api/<version>/topic_stream/<topic_name> Stream a topic over HTTP by keeping the http connection alive. """ topic_name = "/" + topic_name try: msg_class, real_topic, _ = rostopic.get_topic_class(topic_name) except rostopic.ROSTopicIOException as e: raise e if not real_topic: return error("Topic does not exist", 404) queue = Queue(5) def callback(dataIn, queue=queue): data = getattr(dataIn, "data", None) if data is None: data = {"header": getattr(dataIn, "header"), "status": getattr(dataIn, "status")} queue.put(data) sub = rospy.Subscriber(real_topic, msg_class, callback) def gen(queue=queue): while True: x = queue.get() yield str(x) + "\n" return Response(gen(), mimetype='text/plain')
def init_subscribers(self, recipe_phases): ALL_SUBSCRIBER_LIST = { variable.name: rospy.Subscriber( "{}/desired".format(variable.name), get_message_class(variable.type), self.callback) for variable in VALID_VARIABLES } VARIABLES = list(recipe_phases[0]['step'].keys()) # Select the variables in the 1st phase subs = [ALL_SUBSCRIBER_LIST[variable] for variable in VARIABLES] # Initiate all the subscribers for each variable listed in the recipe #air_temp_sub = ALL_SUBSCRIBER_LIST['air_temperature'] #Not working, since the specified recpie name is not in the DB. # def test_run_recipe(self): # self.init_subscribers(MOCK_RECIPE_FLEXFORMAT_A['phases']) # rospy.sleep(10) # Wait for subscribers to spin up # rospy.loginfo(rosservice.get_service_list()) # self.start_recipe(MOCK_RECIPE_FLEXFORMAT_A['_id']) # rospy.sleep(21) #------------------------------------------------------------------------------
def __init__(self): """Constructor for the class initialize topic subscription and instance variables """ self.r = rospy.Rate(5) self.publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10) rospy.Subscriber('/scan', LaserScan, self.process_scan) # user chooses which parking mode to be performed self.is_parallel = rospy.get_param('~parallel', False) # Instance Variables self.timestamp1 = None self.timestamp2 = None self.dist2Neato = None self.dist2Wall = None self.widthOfSpot = None self.twist = None self.radius = None # Adjusment to be made before moving along the arc self.adjustment = 0 self.isAligned = False
def __init__(self): """ Initialize the parking spot recognizer """ #Subscribe to topics of interest rospy.Subscriber("/camera/image_raw", Image, self.process_image) rospy.Subscriber('/cmd_vel', Twist, self.process_twist) # Initialize video images cv2.namedWindow('video_window') self.cv_image = None # the latest image from the camera self.dst = np.zeros(IMG_SHAPE, np.uint8) self.arc_image = np.zeros(IMG_SHAPE, np.uint8) self.transformed = np.zeros(IMG_SHAPE, np.uint8) # Declare instance variables self.bridge = CvBridge() # used to convert ROS messages to OpenCV self.hsv_lb = np.array([0, 70, 60]) # hsv lower bound to filter for parking lines self.hsv_ub = np.array([30, 255, 140]) # hsv upper bound self.vel = None self.omega = None self.color = (0,0,255)
def latch_subscriber(self, step_name, topic, topic_cls, cb): step_dict = self.get_step_dict(step_name) def handle_topic(msg): if self.step == step_name: cb(msg) subs = step_dict.get('subscribers', {}) # Shutdown old timer with same ID if it exists sub = subs.get(topic, None) if sub: sub.unregister() sub = rospy.Subscriber( topic, topic_cls, handle_topic ) subs[topic] = sub
def bumper_callback(self, data): """ Subscriber handler for '/mobile_base/events/bumper'. Assigns own properties based on the BumperState message received. Theses values are assessed in self.perform, where appropriate action is taken under certain circumstances. """ self.bumper_event = data if data.state == 1: EventClient.capture_event(EventTypes.BUMPER, True) else: pass
def cliff_detected_callback(self, data): """ Subscriber handler for '/mobile_base/events/cliff'. Assigns own properties based on the CliffState message received. These values are assessed in self.perform, where appropriate action is taken under certain circumstances. """ self.cliff_states[data.sensor] = data.state if data.state == CliffEvent.CLIFF: # EventClient.capture_event(EventTypes.CLIFF, True) self.ground_unsafe() elif self.safe_on_ground == False: self.check_ground_safe()
def check_kobuki_battery(self, data): """ Subscriber handler that publishes an emergency message if the Kobuki base's batter is low on power. Args: data (kobuki_msgs.msg.SensorState): the current state of the Kobuki base. """ if data.charger == kobuki_msg.SensorState.DISCHARGING: self.discharging = True if data.battery and data.battery < self.battery_threshold: self.kobuki_healthy = False else: self.kobuki_healthy = True
def start_processing(self): """ Start processing data """ if self.disabled: rospy.loginfo("Processing started") self.disabled = False if self.sub_left is None: self.sub_left = rospy.Subscriber( "/multisense/camera/left/image_color", Image, self.left_color_detection) rospy.sleep(0.1) if self.sub_right is None: self.sub_right = rospy.Subscriber( "/multisense/camera/right/image_color", Image, self.right_color_detection) rospy.sleep(0.1) if self.sub_cloud is None: self.sub_cloud = rospy.Subscriber( "/multisense/image_points2_color", PointCloud2, self.stereo_cloud)
def run(self, distance, angle, snap_to, options): """ Run our code """ rospy.loginfo("Start test code") self.task_subscriber = rospy.Subscriber("/srcsim/finals/task", Task, self.task_status) rate = rospy.Rate(1) # 10hz if self.task_subscriber.get_num_connections() == 0: rospy.loginfo('waiting for task publisher...') while self.task_subscriber.get_num_connections() == 0: rate.sleep() if distance > 0.0001 or distance < -0.005: self.zarj_os.walk.forward(distance, True) while not self.zarj_os.walk.walk_is_done(): rospy.sleep(0.01) if abs(angle) > 0.0 or "turn" in options: align = "align" in options small_forward = 0.005 if align else None self.zarj_os.walk.turn(angle, snap_to, align_to_snap = align, small_forward = small_forward) while not self.zarj_os.walk.walk_is_done(): rospy.sleep(0.01)
def __init__(self): #init code rospy.init_node("robotGame") self.currentDist = 1 self.previousDist = 1 self.reached = False self.tf = TransformListener() self.left_joint_names = ['yumi_joint_1_l', 'yumi_joint_2_l', 'yumi_joint_3_l', 'yumi_joint_4_l', 'yumi_joint_5_l', 'yumi_joint_6_l', 'yumi_joint_7_l', 'gripper_l_joint'] self.right_joint_names = ['yumi_joint_1_r', 'yumi_joint_2_r', 'yumi_joint_3_r', 'yumi_joint_4_r', 'yumi_joint_5_r', 'yumi_joint_6_r', 'yumi_joint_7_r', 'gripper_r_joint'] self.left_positions = [-2.01081820427463881, 1.4283937236421274, -1.3593418228836045, -0.19315625641494183, 1.7016501799872579, 0.6573540231496411, 3.404315594906305, 0.0] self.right_positions = [0.01081820427463881, 2.4283937236421274, 0.3593418228836045, -0.19315625641494183, 1.7016501799872579, 0.6573540231496411, 3.404315594906305, 1.8145107750466565] self.rjv = [] self.ljv = [] self.pub = rospy.Publisher('my_joint_states', JointState, queue_size=1) self.js = JointState() self.js.header = Header() self.js.name = self.left_joint_names + self.right_joint_names self.js.velocity = [] self.js.effort = [] self.sub = rospy.Subscriber('/joint_states', JointState, self.jsCB) self.destPos = np.random.uniform(0,0.25, size =(3)) self.reset()
def main(): rospy.init_node("evaluation_ac") ac = ACControllerSimulator() rospy.Subscriber("/aide/ac_control", Bool, callback=lambda msg: ac.increase_ac() if msg.data else ac.decrease_ac()) console = Console() console.preprocess = lambda source: source[3:] atexit.register(loginfo, "Going down by user-input.") ac_thread = Thread(target=ac.simulate) ac_thread.daemon = True pub_thread = Thread(target=publish, args=(ac, )) pub_thread.daemon = True pub_thread.start() while not rospy.is_shutdown(): try: command = console.raw_input("ac> ") if command == "start": ac_thread.start() if command == "end": return except EOFError: print("") ac.finished = True rospy.signal_shutdown("Going down.")
def pub_sub_local_legacy(self): self.pub["motor"] = rospy.Publisher("/motor", Float32MultiArray) # learning signals # FIXME: change these names to /learner/... self.pub["learn_dw"] = rospy.Publisher("/learner/dw", Float32MultiArray) self.pub["learn_w"] = rospy.Publisher("/learner/w", Float32MultiArray) self.pub["learn_perf"] = rospy.Publisher("/learner/perf", reservoir) self.pub["learn_perf_lp"] = rospy.Publisher("/learner/perf_lp", reservoir) # learning control self.sub["ctrl_eta"] = rospy.Subscriber("/learner/ctrl/eta", Float32, self.sub_cb_ctrl) self.sub["ctrl_theta"] = rospy.Subscriber("/learner/ctrl/theta", Float32, self.sub_cb_ctrl) self.sub["ctrl_target"] = rospy.Subscriber("/learner/ctrl/target", Float32, self.sub_cb_ctrl) # state self.pub["learn_x_raw"] = rospy.Publisher("/learner/x_raw", reservoir) self.pub["learn_x"] = rospy.Publisher("/learner/x", reservoir) self.pub["learn_r"] = rospy.Publisher("/learner/r", reservoir) self.pub["learn_y"] = rospy.Publisher("/learner/y", reservoir)
def __init__(self, limb_name, topic='/sensor_values'): super(FingerSensorBaxter, self).__init__(limb_name) self.listen = tf.TransformListener() self.inside = np.zeros(14) self.tip = np.zeros(2) self.inside_offset = np.zeros_like(self.inside) self.tip_offset = np.zeros_like(self.tip) # Picking level self.level = None self.last_sensor_update = 0 self.sensor_sub = rospy.Subscriber(topic, Int32MultiArray, self.update_sensor_values, queue_size=1) self.object_frame = "" self.camera_centroids = np.zeros((num_cubes, 3)) self.touch_centroids = np.zeros((num_cubes, 3)) self.current_index = 0 self.update_transform = rospy.Publisher("/update_camera_alignment", Pose, queue_size = 1) #self.zero_sensor()
def __init__(self, limb_name, topic='/sensor_values'): super(FingerSensorBaxter, self).__init__(limb_name) self.listen = tf.TransformListener() self.inside = np.zeros(14) self.tip = np.zeros(2) self.inside_offset = np.zeros_like(self.inside) self.tip_offset = np.zeros_like(self.tip) # Picking level self.level = None self.last_sensor_update = 0 self.sensor_sub = rospy.Subscriber(topic, Int32MultiArray, self.update_sensor_values, queue_size=1) self.object_frame = "" self.perc_centroids = np.array([]) self.touch_centroids = np.array([]) self.update_transform = rospy.Publisher("/update_camera_alignment", Pose, queue_size = 1) self.handle_found = False #self.zero_sensor() #import ipdb;ipdb.set_trace()
