Python源码示例：rospy.Subscriber()
示例1
def __init__(self):
"""
Create HeadLiftJoy object.
"""
# params
self._head_axes = rospy.get_param('~head_axes', 3)
self._lift_axes = rospy.get_param('~lift_axes', 3)
self._head_button = rospy.get_param('~head_button', 4)
self._lift_button = rospy.get_param('~lift_button', 5)
# pubs
self._head_pub = rospy.Publisher('head_angle', Float64, queue_size=1)
self._lift_pub = rospy.Publisher('lift_height', Float64, queue_size=1)
# subs
self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_cb, queue_size=1)
示例2
def __init__(self, goalListX, goalListY, retry, map_frame):
self.sub = rospy.Subscriber('move_base/result', MoveBaseActionResult, self.statusCB, queue_size=10)
self.pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10)
# params & variables
self.goalListX = goalListX
self.goalListY = goalListY
self.retry = retry
self.goalId = 0
self.goalMsg = PoseStamped()
self.goalMsg.header.frame_id = map_frame
self.goalMsg.pose.orientation.z = 0.0
self.goalMsg.pose.orientation.w = 1.0
# Publish the first goal
time.sleep(1)
self.goalMsg.header.stamp = rospy.Time.now()
self.goalMsg.pose.position.x = self.goalListX[self.goalId]
self.goalMsg.pose.position.y = self.goalListY[self.goalId]
self.pub.publish(self.goalMsg)
rospy.loginfo("Initial goal published! Goal ID is: %d", self.goalId)
self.goalId = self.goalId + 1
示例3
def __init__(self):
#############################################################
rospy.init_node("twist_to_motors")
nodename = rospy.get_name()
rospy.loginfo("%s started" % nodename)
self.w = rospy.get_param("~base_width", 0.2)
self.pub_lmotor = rospy.Publisher('lwheel_vtarget', Float32, queue_size=10)
self.pub_rmotor = rospy.Publisher('rwheel_vtarget', Float32, queue_size=10)
rospy.Subscriber('twist', Twist, self.twistCallback)
self.rate = rospy.get_param("~rate", 50)
self.timeout_ticks = rospy.get_param("~timeout_ticks", 2)
self.left = 0
self.right = 0
#############################################################
示例4
def __init__(self):
###############################################
rospy.init_node("wheel_loopback");
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
self.rate = rospy.get_param("~rate", 200)
self.timeout_secs = rospy.get_param("~timeout_secs", 0.5)
self.ticks_meter = float(rospy.get_param('~ticks_meter', 50))
self.velocity_scale = float(rospy.get_param('~velocity_scale', 255))
self.latest_motor = 0
self.wheel = 0
rospy.Subscriber('motor', Int16, self.motor_callback)
self.pub_wheel = rospy.Publisher('wheel', Int16, queue_size=10)
###############################################
示例5
def scaleWheel():
rospy.init_node("wheel_scaler")
rospy.loginfo("wheel_scaler started")
scale = rospy.get_param('distance_scale', 1)
rospy.loginfo("wheel_scaler scale: %0.2f", scale)
rospy.Subscriber("lwheel", Int16, lwheelCallback)
rospy.Subscriber("rwheel", Int16, rwheelCallback)
lscaled_pub = rospy.Publisher("lwheel_scaled", Int16, queue_size=10)
rscaled_pub = rospy.Publisher("rwheel_scaled", Int16, queue_size=10)
### testing sleep CPU usage
r = rospy.Rate(50)
while not rospy.is_shutdown:
r.sleep()
rospy.spin()
示例6
def talker_ctrl():
global rate_value
rospy.init_node('usv_simple_ctrl', anonymous=True)
rate = rospy.Rate(rate_value) # 10h
# publishes to thruster and rudder topics
pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
# subscribe to state and targer point topics
rospy.Subscriber("state", Odometry, get_pose) # get usv position (add 'gps' position latter)
rospy.Subscriber("move_usv/goal", Odometry, get_target) # get target position
while not rospy.is_shutdown():
try:
pub_motor.publish(thruster_ctrl_msg())
pub_rudder.publish(rudder_ctrl_msg())
pub_result.publish(verify_result())
rate.sleep()
except rospy.ROSInterruptException:
rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
except rospy.ROSTimeMovedBackwardsException:
rospy.logerr("ROS Time Backwards! Just ignore the exception!")
示例7
def talker_ctrl():
global rate_value
global result
# publishes to thruster and rudder topics
pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
rospy.init_node('usv_simple_ctrl', anonymous=True)
rate = rospy.Rate(rate_value) # 10h
# subscribe to state and targer point topics
rospy.Subscriber("state", Odometry, get_pose) # get usv position (add 'gps' position latter)
rospy.Subscriber("move_usv/goal", Odometry, get_target) # get target position
while not rospy.is_shutdown():
try:
pub_motor.publish(thruster_ctrl_msg())
pub_result.publish(verify_result())
rate.sleep()
except rospy.ROSInterruptException:
rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
except rospy.ROSTimeMovedBackwardsException:
rospy.logerr("ROS Time Backwards! Just ignore the exception!")
示例8
def talker_ctrl():
global rate_value
rospy.init_node('usv_simple_ctrl', anonymous=True)
rate = rospy.Rate(rate_value) # 10h
# publishes to thruster and rudder topics
pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
# subscribe to state and targer point topics
rospy.Subscriber("state", Odometry, get_pose) # get usv position (add 'gps' position latter)
rospy.Subscriber("move_usv/goal", Odometry, get_target) # get target position
while not rospy.is_shutdown():
try:
pub_motor.publish(thruster_ctrl_msg())
pub_rudder.publish(rudder_ctrl_msg())
pub_result.publish(verify_result())
rate.sleep()
except rospy.ROSInterruptException:
rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
except rospy.ROSTimeMovedBackwardsException:
rospy.logerr("ROS Time Backwards! Just ignore the exception!")
示例9
def __init__(self):
rospy.loginfo("Tianbot Racecar JoyTeleop Initializing...")
self._twist = Twist()
self._twist.linear.x = 1500
self._twist.angular.z = 90
self._deadman_pressed = False
self._zero_twist_published = False
self._cmd_vel_pub = rospy.Publisher('~/car/cmd_vel', Twist, queue_size=5)
self._joy_sub = rospy.Subscriber('/joy', Joy, self.joy_callback)
self._timer = rospy.Timer(rospy.Duration(0.05), self.joystick_controller)
self._axis_throttle = 1
_joy_mode = rospy.get_param("~joy_mode", "D").lower()
if _joy_mode == "d":
self._axis_servo = 2
if _joy_mode == "x":
self._axis_servo = 3
self._throttle_scale = rospy.get_param("~throttle_scale", 0.5)
self._servo_scale = rospy.get_param("~servo_scale", 1)
示例10
def __init__(self):
super(WSG50Gripper, self).__init__()
self.max_release = 0
self.sem_list = [Semaphore(value = 0)]
self._status_mutex = Lock()
self._desired_gpos = GRIPPER_OPEN
self._gripper_speed = 300
self._force_counter = 0
self._integrate_gripper_force, self._last_integrate = 0., None
self._last_status_t = time.time()
self.num_timeouts = 0
self.gripper_pub = rospy.Publisher('/wsg_50_driver/goal_position', Cmd, queue_size=10)
rospy.Subscriber("/wsg_50_driver/status", Status, self._gripper_callback)
logging.getLogger('robot_logger').info("waiting for first status")
self.sem_list[0].acquire()
logging.getLogger('robot_logger').info('gripper initialized!')
self._bg = Thread(target=self._background_monitor)
self._bg.start()
示例11
def __init__(self, robot_name, print_debug, email_cred_file='', log_file='', control_rate=100, gripper_attached='default'):
super(WidowXController, self).__init__(robot_name, print_debug, email_cred_file, log_file, control_rate, gripper_attached)
self._redist_rate = rospy.Rate(50)
self._arbotix = ArbotiX('/dev/ttyUSB0')
assert self._arbotix.syncWrite(MAX_TORQUE_L, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring"
assert self._arbotix.syncWrite(TORQUE_LIMIT, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring"
self._joint_lock = Lock()
self._angles, self._velocities = {}, {}
rospy.Subscriber("/joint_states", JointState, self._joint_callback)
time.sleep(1)
self._joint_pubs = [rospy.Publisher('/{}/command'.format(name), Float64, queue_size=1) for name in JOINT_NAMES[:-1]]
self._gripper_pub = rospy.Publisher('/gripper_prismatic_joint/command', Float64, queue_size=1)
p.connect(p.DIRECT)
widow_x_urdf = '/'.join(__file__.split('/')[:-1]) + '/widowx/widowx.urdf'
self._armID = p.loadURDF(widow_x_urdf, useFixedBase=True)
p.resetBasePositionAndOrientation(self._armID, [0, 0, 0], p.getQuaternionFromEuler([np.pi, np.pi, np.pi]))
self._n_errors = 0
示例12
def __init__(self, topic_data, opencv_tracking=False, save_videos=False):
self._tracking_enabled, self._save_vides = opencv_tracking, save_videos
self._latest_image = LatestObservation(self._tracking_enabled, self._save_vides)
self._is_tracking = False
if self._tracking_enabled:
self.box_height = 80
self.bridge = CvBridge()
self._is_first_status, self._status_sem = True, Semaphore(value=0)
self._cam_height, self._cam_width = None, None
self._last_hash, self._num_repeats = None, 0
if self._save_vides:
self._buffers = []
self._saving = False
self._topic_data = topic_data
self._image_dtype = topic_data.dtype
rospy.Subscriber(topic_data.name, Image_msg, self.store_latest_im)
logging.getLogger('robot_logger').debug('downing sema on topic: {}'.format(topic_data.name))
self._status_sem.acquire()
logging.getLogger('robot_logger').info("Cameras {} subscribed: stream is {}x{}".format(self._topic_data.name, self._cam_width, self._cam_height))
示例13
def __init__(self):
#############################################################
rospy.init_node("twist_to_motors")
nodename = rospy.get_name()
rospy.loginfo("%s started" % nodename)
self.w = rospy.get_param("~base_width", 0.2)
self.pub_lmotor = rospy.Publisher('lwheel_vtarget', Float32,queue_size=10)
self.pub_rmotor = rospy.Publisher('rwheel_vtarget', Float32,queue_size=10)
rospy.Subscriber('cmd_vel_mux/input/teleop', Twist, self.twistCallback)
self.rate = rospy.get_param("~rate", 50)
self.timeout_ticks = rospy.get_param("~timeout_ticks", 2)
self.left = 0
self.right = 0
#############################################################
示例14
def __init__(self):
###############################################
rospy.init_node("wheel_loopback");
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
self.rate = rospy.get_param("~rate", 200)
self.timeout_secs = rospy.get_param("~timeout_secs", 0.5)
self.ticks_meter = float(rospy.get_param('~ticks_meter', 50))
self.velocity_scale = float(rospy.get_param('~velocity_scale', 255))
self.latest_motor = 0
self.wheel = 0
rospy.Subscriber('motor', Int16, self.motor_callback)
self.pub_wheel = rospy.Publisher('wheel', Int16,queue_size=10)
###############################################
示例15
def listener():
global obj
global pub
rospy.loginfo("Starting prediction node")
rospy.init_node('listener', anonymous = True)
rospy.Subscriber("sensor_read", Int32, read_sensor_data)
pub = rospy.Publisher('predict', Int32, queue_size=1)
obj = Classify_Data()
obj.read_file('pos_readings.csv')
obj.train()
rospy.spin()
示例16
def __init__(self):
#############################################################
rospy.init_node("twist_to_motors")
nodename = rospy.get_name()
rospy.loginfo("%s started" % nodename)
self.w = rospy.get_param("~base_width", 0.2)
self.pub_lmotor = rospy.Publisher('lwheel_vtarget', Float32,queue_size=10)
self.pub_rmotor = rospy.Publisher('rwheel_vtarget', Float32,queue_size=10)
rospy.Subscriber('cmd_vel_mux/input/teleop', Twist, self.twistCallback)
self.rate = rospy.get_param("~rate", 50)
self.timeout_ticks = rospy.get_param("~timeout_ticks", 2)
self.left = 0
self.right = 0
#############################################################
示例17
def __init__(self):
###############################################
rospy.init_node("wheel_loopback");
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
self.rate = rospy.get_param("~rate", 200)
self.timeout_secs = rospy.get_param("~timeout_secs", 0.5)
self.ticks_meter = float(rospy.get_param('~ticks_meter', 50))
self.velocity_scale = float(rospy.get_param('~velocity_scale', 255))
self.latest_motor = 0
self.wheel = 0
rospy.Subscriber('motor', Int16, self.motor_callback)
self.pub_wheel = rospy.Publisher('wheel', Int16,queue_size=10)
###############################################
示例18
def __init__(self):
#############################################################
rospy.init_node("twist_to_motors")
nodename = rospy.get_name()
rospy.loginfo("%s started" % nodename)
self.w = rospy.get_param("~base_width", 0.2)
self.pub_lmotor = rospy.Publisher('lwheel_vtarget', Float32,queue_size=10)
self.pub_rmotor = rospy.Publisher('rwheel_vtarget', Float32,queue_size=10)
rospy.Subscriber('cmd_vel_mux/input/teleop', Twist, self.twistCallback)
self.rate = rospy.get_param("~rate", 50)
self.timeout_ticks = rospy.get_param("~timeout_ticks", 2)
self.left = 0
self.right = 0
#############################################################
示例19
def __init__(self):
###############################################
rospy.init_node("wheel_loopback");
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
self.rate = rospy.get_param("~rate", 200)
self.timeout_secs = rospy.get_param("~timeout_secs", 0.5)
self.ticks_meter = float(rospy.get_param('~ticks_meter', 50))
self.velocity_scale = float(rospy.get_param('~velocity_scale', 255))
self.latest_motor = 0
self.wheel = 0
rospy.Subscriber('motor', Int16, self.motor_callback)
self.pub_wheel = rospy.Publisher('wheel', Int16,queue_size=10)
###############################################
示例20
def listener():
rospy.init_node('serial_adapter', anonymous=True)
# Declare the Subscriber to motors orders
rospy.Subscriber("arduino/servo", Int16, servoCallback, queue_size=2)
rospy.Subscriber("arduino/motor", Int8, motorCallback, queue_size=2)
rospy.spin()
示例21
def talker_ctrl():
global rate_value
global currentHeading
global windDir
global isTacking
global heeling
global spHeading
rospy.init_node('usv_simple_ctrl', anonymous=True)
rate = rospy.Rate(rate_value) # 10h
# publishes to thruster and rudder topics
#pub_sail = rospy.Publisher('angleLimits', Float64, queue_size=10)
pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
pub_heading = rospy.Publisher('currentHeading', Float64, queue_size=10)
pub_windDir = rospy.Publisher('windDirection', Float64, queue_size=10)
pub_heeling = rospy.Publisher('heeling', Float64, queue_size=10)
pub_spHeading = rospy.Publisher('spHeading', Float64, queue_size=10)
# subscribe to state and targer point topics
rospy.Subscriber("state", Odometry, get_pose) # get usv position (add 'gps' position latter)
rospy.Subscriber("move_usv/goal", Odometry, get_target) # get target position
while not rospy.is_shutdown():
try:
pub_rudder.publish(rudder_ctrl_msg())
#pub_sail.publish(sail_ctrl())
pub_result.publish(verify_result())
pub_heading.publish(currentHeading)
pub_windDir.publish(windDir)
pub_heeling.publish(heeling)
pub_spHeading.publish(spHeading)
rate.sleep()
except rospy.ROSInterruptException:
rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
except rospy.ROSTimeMovedBackwardsException:
rospy.logerr("ROS Time Backwards! Just ignore the exception!")
示例22
def __init__(self):
rospy.init_node('usv_vel_ctrl', anonymous=False)
self.rate = 10
r = rospy.Rate(self.rate) # 10hertz
self.usv_vel = Odometry()
self.usv_vel_ant = Odometry()
self.target_vel = Twist()
self.target_vel_ant = Twist()
self.thruster_msg = JointState()
self.rudder_msg = JointState()
self.thruster_msg.header = Header()
self.rudder_msg.header = Header()
self.kp_lin = 80
self.ki_lin = 200
thruster_name = 'fwd'
rudder_name = 'rudder_joint'
self.I_ant_lin = 0
self.I_ant_ang = 0
self.lin_vel = 0
self.lin_vel_ang = 0
self.ang_vel = 0
self.kp_ang = 2
self.ki_ang = 4
self.rudder_max = 70
self.thruster_max = 30
self.pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
self.pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
rospy.Subscriber("state", Odometry, self.get_usv_vel)
rospy.Subscriber("cmd_vel", Twist, self.get_target_vel)
while not rospy.is_shutdown():
self.pub_rudder.publish(self.rudder_ctrl_msg(self.ang_vel_ctrl(), rudder_name))
self.pub_motor.publish(self.thruster_ctrl_msg(self.lin_vel_ctrl(), thruster_name))
r.sleep()
示例23
def __init__(self):
rospy.init_node('usv_vel_ctrl', anonymous=False)
self.rate = 10
r = rospy.Rate(self.rate) # 10hertz
self.usv_vel = Odometry()
self.usv_vel_ant = Odometry()
self.target_vel = Twist()
self.target_vel_ant = Twist()
self.thruster_msg = JointState()
self.thruster_msg.header = Header()
self.kp_lin = 80
self.ki_lin = 200
thruster_name = 'fwd_left, fwd_right'
self.I_ant_lin = 0
self.I_ant_ang = 0
self.lin_vel = 0
self.lin_vel_ang = 0
self.ang_vel = 0
self.kp_ang = 80
self.ki_ang = 100
self.thruster_max = 30
self.vel_left = 0
self.vel_right = 0
self.thruster_command = numpy.array([0, 0])
self.erro = 0
self.pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
rospy.Subscriber("state", Odometry, self.get_usv_vel)
rospy.Subscriber("cmd_vel", Twist, self.get_target_vel)
while not rospy.is_shutdown():
self.pub_motor.publish(self.thruster_ctrl_msg(self.vel_ctrl(), thruster_name))
r.sleep()
示例24
def talker_ctrl():
# publishes to thruster and rudder topics
pub_motor = rospy.Publisher('/barco_auv/thruster_command', JointState, queue_size=10)
pub_rudder = rospy.Publisher('/barco_auv/joint_setpoint', JointState, queue_size=10)
rospy.init_node('usv_simple_ctrl', anonymous=True)
rate = rospy.Rate(10) # 10h
# subscribe to state and targer point topics
rospy.Subscriber("/barco_auv/state", Odometry, get_pose) # get usv position (add 'gps' position latter)
rospy.Subscriber("usv_position_setpoint", Odometry, get_target) # get target position
while not rospy.is_shutdown():
pub_motor.publish(thruster_ctrl_msg())
pub_rudder.publish(rudder_ctrl_msg())
rate.sleep()
示例25
def talker_ctrl():
global rate_value
global currentHeading
global windDir
rospy.init_node('usv_simple_ctrl', anonymous=True)
rate = rospy.Rate(rate_value) # 10h
# publishes to thruster and rudder topics
#pub_sail = rospy.Publisher('angleLimits', Float64, queue_size=10)
pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
pub_heading= rospy.Publisher('currentHeading', Float64, queue_size=10)
pub_windDir= rospy.Publisher('windDirection', Float64, queue_size=10)
# subscribe to state and targer point topics
rospy.Subscriber("state", Odometry, get_pose) # get usv position (add 'gps' position latter)
rospy.Subscriber("move_usv/goal", Odometry, get_target) # get target position
while not rospy.is_shutdown():
try:
pub_rudder.publish(rudder_ctrl_msg())
#pub_sail.publish(sail_ctrl())
pub_result.publish(verify_result())
pub_heading.publish(currentHeading)
pub_windDir.publish(windDir)
rate.sleep()
except rospy.ROSInterruptException:
rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
except rospy.ROSTimeMovedBackwardsException:
rospy.logerr("ROS Time Backwards! Just ignore the exception!")
示例26
def startRosService():
rospy.Subscriber("waterCurrentTime", Int64, defineTime)
preprocessDataset2()
s = rospy.Service('waterCurrent', GetSpeed, handleWaterCurrent2)
print "\nReady to answer water current.\n"
示例27
def startTopicChangeTime():
rospy.Subscriber("windCurrentTime", Int64, defineTime)
print "Ready to change time."
示例28
def __init__(self):
# Create a publisher for our custom message.
self.pub = rospy.Publisher('from_ros_to_slack', String, queue_size=10)
rospy.Subscriber("from_slack_to_ros", String, self.callback)
# Main while loop.
while not rospy.is_shutdown():
# Sleep for a while before publishing new messages. Division is so rate != period.
rospy.sleep(5.0)
示例29
def __init__(self):
# Get the ~private namespace parameters from command line or launch file.
self.token = rospy.get_param('~token', 'xoxp-123456789')
self.channel = rospy.get_param('~channel', 'G1234567Q')
self.username = rospy.get_param('~username', 'ros-bot')
# Create a publisher for our custom message.
pub = rospy.Publisher('from_slack_to_ros', String, queue_size=10)
rospy.Subscriber("from_ros_to_slack", String, self.callback)
rospy.Subscriber("send_file_to_slack", String, self.filecallback)
# Create the slack client
self.sc = SlackClient(self.token)
if self.sc.rtm_connect():
# Main while loop.
while not rospy.is_shutdown():
for reply in self.sc.rtm_read():
if "type" in reply and "user" in reply:
#print reply
if reply["type"] == "message" and reply["channel"] == self.channel:
pub.publish(reply["text"])
# Sleep for a while before publishing new messages. Division is so rate != period.
rospy.sleep(2.0)
示例30
def __init__(self, image_topic):
self.image_sub = rospy.Subscriber(image_topic, Image, self.callback_image, queue_size=1)
self.bridge = CvBridge()
self.frames = []
