Python源码示例：rospy.init_node()
示例1
def talker_ctrl():
global rate_value
# 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)
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("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()
示例2
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
#############################################################
示例3
def main():
##########################################################################
##########################################################################
rospy.init_node('virtual_joystick')
rospy.loginfo('virtual_joystick started')
global x_min
global x_max
global r_min
global r_max
x_min = rospy.get_param("~x_min", -0.20)
x_max = rospy.get_param("~x_max", 0.20)
r_min = rospy.get_param("~r_min", -1.0)
r_max = rospy.get_param("~r_max", 1.0)
app = QtGui.QApplication(sys.argv)
ex = MainWindow()
sys.exit(app.exec_())
示例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 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!")
示例6
def navigation():
pub = rospy.Publisher('usv_position_setpoint', Odometry, queue_size=10) # only create a rostopic, navigation system TODO
rospy.init_node('navigation_publisher')
rate = rospy.Rate(60) # 10h
x = -20.0
y = -20.0
msg = Odometry()
# msg.header = Header()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "navigation"
msg.pose.pose.position = Point(x, y, 0.)
while not rospy.is_shutdown():
pub.publish(msg)
rate.sleep()
示例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 start(self, node_name='polly_node', service_name='polly'):
"""The entry point of a ROS service node.
Details of the service API can be found in Polly.srv.
:param node_name: name of ROS node
:param service_name: name of ROS service
:return: it doesn't return
"""
rospy.init_node(node_name)
service = rospy.Service(service_name, Polly, self._node_request_handler)
rospy.loginfo('polly running: {}'.format(service.uri))
rospy.spin()
示例10
def __init__(self, args):
self.index = 0
if len(sys.argv) > 1:
self.index = int(sys.argv[1])
rospy.init_node('save_img')
self.bridge = CvBridge()
while not rospy.is_shutdown():
raw_input()
data = rospy.wait_for_message('/camera1/usb_cam/image_raw', Image)
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "passthrough")
except CvBridgeError as e:
print(e)
print "Saved to: ", base_path+str(self.index)+".jpg"
cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB, cv_image)
cv2.imwrite(join(base_path, "frame{:06d}.jpg".format(self.index)), cv_image)#*255)
self.index += 1
rospy.spin()
示例11
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
#############################################################
示例12
def main():
##########################################################################
##########################################################################
rospy.init_node('virtual_joystick')
rospy.loginfo('virtual_joystick started')
global x_min
global x_max
global r_min
global r_max
x_min = rospy.get_param("~x_min", -0.20)
x_max = rospy.get_param("~x_max", 0.20)
r_min = rospy.get_param("~r_min", -1.0)
r_max = rospy.get_param("~r_max", 1.0)
app = QtGui.QApplication(sys.argv)
ex = MainWindow()
sys.exit(app.exec_())
示例13
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()
示例14
def main():
##########################################################################
##########################################################################
rospy.init_node('virtual_joystick')
rospy.loginfo('virtual_joystick started')
global x_min
global x_max
global r_min
global r_max
x_min = rospy.get_param("~x_min", -0.20)
x_max = rospy.get_param("~x_max", 0.20)
r_min = rospy.get_param("~r_min", -1.0)
r_max = rospy.get_param("~r_max", 1.0)
app = QtGui.QApplication(sys.argv)
ex = MainWindow()
sys.exit(app.exec_())
示例15
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)
###############################################
示例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 main():
##########################################################################
##########################################################################
rospy.init_node('virtual_joystick')
rospy.loginfo('virtual_joystick started')
global x_min
global x_max
global r_min
global r_max
x_min = rospy.get_param("~x_min", -0.20)
x_max = rospy.get_param("~x_max", 0.20)
r_min = rospy.get_param("~r_min", -1.0)
r_max = rospy.get_param("~r_max", 1.0)
app = QtGui.QApplication(sys.argv)
ex = MainWindow()
sys.exit(app.exec_())
示例18
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()
示例19
def talker():
pub = rospy.Publisher('/barco_auv/thruster_command', JointState, queue_size=10)
rospy.init_node('ctrl_jointState_publisher', anonymous=True)
rate = rospy.Rate(10) # 10hz
hello_str = JointState()
hello_str.header = Header()
hello_str.name = ['fwd_left']
hello_str.position = [10]
hello_str.velocity = []
hello_str.effort = []
while not rospy.is_shutdown():
rospy.loginfo(hello_str)
pub.publish(hello_str)
rate.sleep()
示例20
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!")
示例21
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()
示例22
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()
示例23
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!")
示例24
def start(self, node_name='synthesizer_node', service_name='synthesizer'):
"""The entry point of a ROS service node.
:param node_name: name of ROS node
:param service_name: name of ROS service
:return: it doesn't return
"""
rospy.init_node(node_name)
service = rospy.Service(service_name, Synthesizer, self._node_request_handler)
rospy.loginfo('{} running: {}'.format(node_name, service.uri))
rospy.spin()
示例25
def main():
rospy.init_node('Demo_Stats')
rs = intera_interface.RobotEnable(CHECK_VERSION)
state = rs.state()
print state
rospy.signal_shutdown("Demo_Stats finished.")
示例26
def main():
"""RSDK Gripper Example: send a command to control the grippers.
Run this example to command various gripper movements while
adjusting gripper parameters, including calibration, and velocity:
Uses the intera_interface.Gripper class and the
helper function, intera_external_devices.getch.
"""
epilog = """
See help inside the example with the '?' key for key bindings.
"""
rp = intera_interface.RobotParams()
valid_limbs = rp.get_limb_names()
if not valid_limbs:
rp.log_message(("Cannot detect any limb parameters on this robot. "
"Exiting."), "ERROR")
return
limb = valid_limbs[0]
print("Using limb: {}.".format(limb))
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__,
epilog=epilog)
parser.add_argument(
"-a", "--action", dest="action", default="open",
choices=["open", "close"],
help="Action to perform with the gripper. Options: close or open"
)
args = parser.parse_args(rospy.myargv()[1:])
print("Initializing node... ")
rospy.init_node("sdk_gripper_keyboard")
move_gripper(limb, args.action)
示例27
def test_right_hand_ros():
"""
Test the cube orientation sensing using ROS
"""
rospy.init_node('cv_detection_right_hand_camera')
camera_name = "right_hand_camera"
camera_helper = CameraHelper(camera_name, "base", 0)
bridge = CvBridge()
try:
while not rospy.is_shutdown():
# Take picture
img_data = camera_helper.take_single_picture()
# Convert to OpenCV format
cv_image = bridge.imgmsg_to_cv2(img_data, "bgr8")
# Save for debugging
#cv2.imwrite("/tmp/debug.png", cv_image)
# Get cube rotation
angles = get_cubes_z_rotation(cv_image)
print(angles)
# Wait for a key press
cv2.waitKey(1)
rospy.sleep(0.1)
except CvBridgeError, err:
rospy.logerr(err)
# Exit
示例28
def start_server(gripper):
print("Initializing node... ")
rospy.init_node("rsdk_gripper_action_server%s" %
("" if gripper == 'both' else "_" + gripper,))
print("Initializing gripper action server...")
gas = GripperActionServer()
print("Running. Ctrl-c to quit")
rospy.spin()
示例29
def __init__(self):
super(PeopleObjectDetectionNode, self).__init__()
# init the node
rospy.init_node('people_object_detection', anonymous=False)
# Get the parameters
(model_name, num_of_classes, label_file, camera_namespace, video_name,
num_workers) \
= self.get_parameters()
# Create Detector
self._detector = Detector(model_name, num_of_classes, label_file,
num_workers)
self._bridge = CvBridge()
# Advertise the result of Object Detector
self.pub_detections = rospy.Publisher('/object_detection/detections', \
DetectionArray, queue_size=1)
# Advertise the result of Object Detector
self.pub_detections_image = rospy.Publisher(\
'/object_detection/detections_image', Image, queue_size=1)
if video_name == "no":
# Subscribe to the face positions
self.sub_rgb = rospy.Subscriber(camera_namespace,\
Image, self.rgb_callback, queue_size=1, buff_size=2**24)
else:
self.read_from_video(video_name)
# spin
rospy.spin()
示例30
def __init__(self):
super(FaceRecognitionNode, self).__init__()
# init the node
rospy.init_node('face_recognition_node', anonymous=False)
# Get the parameters
(image_topic, detection_topic, output_topic, output_topic_rgb) \
= self.get_parameters()
self._bridge = CvBridge()
# Advertise the result of Object Tracker
self.pub_det = rospy.Publisher(output_topic, \
DetectionArray, queue_size=1)
self.pub_det_rgb = rospy.Publisher(output_topic_rgb, \
Image, queue_size=1)
self.sub_detection = message_filters.Subscriber(detection_topic, \
DetectionArray)
self.sub_image = message_filters.Subscriber(image_topic, Image)
# Scaling factor for face recognition image
self.scaling_factor = 1.0
# Read the images from folder and create a database
self.database = self.initialize_database()
ts = message_filters.ApproximateTimeSynchronizer(\
[self.sub_detection, self.sub_image], 2, 0.3)
ts.registerCallback(self.detection_callback)
# spin
rospy.spin()
