def init_ros():
''' ROS node initialization.
Initialize the node, advertise topics, and any other ROS housekeeping.
'''
global topics, param_handler
rospy.init_node('sickldmrs', log_level=rospy.DEBUG)
node_name = rospy.get_name().split('/')[-1]
rospy.loginfo('node %s starting up.' % node_name)
topics['cloud'] = rospy.Publisher('/%s/cloud' % node_name,
numpy_msg(PointCloud2))
topics['scan0'] = rospy.Publisher('/%s/scan0' % node_name,
numpy_msg(LaserScan))
topics['scan1'] = rospy.Publisher('/%s/scan1' % node_name,
numpy_msg(LaserScan))
topics['scan2'] = rospy.Publisher('/%s/scan2' % node_name,
numpy_msg(LaserScan))
topics['scan3'] = rospy.Publisher('/%s/scan3' % node_name,
numpy_msg(LaserScan))
rospy.on_shutdown(node_shutdown_hook)
def __init__(self, vel_output_topic_type, max_speed, ctrl_p, def_stiffness):
#Variable that checks if we can start working
self.configured = False
#Will only start controlling after receiving at least one desired position
self.received_des_pos_values = False
self.def_stiffness = def_stiffness
self.def_damping = 0.7
#Controller parameters
self.ctrl_p = ctrl_p
self.max_speed = max_speed
self.num_dof = 0;
self.init_work_variables()
#Chose the appropriate topic type
if (vel_output_topic_type == 'MultiJointVelocityImpedanceCommand'):
self.vel_output_topic_type = MultiJointVelocityImpedanceCommand
elif (vel_output_topic_type == 'MultiJointVelocityCommand'):
self.vel_output_topic_type = MultiJointVelocityCommand
else:
rospy.logfatal('%s: Velocity output topic type not supported. Extend the JController class if you want to support output to a topic of type %s.' %(rospy.get_name(), vel_output_topic_type))
raise Exception('should_exit')
self.vel_pub = rospy.Publisher("~vel_command_out", numpy_msg(self.vel_output_topic_type), queue_size=3, tcp_nodelay=True, latch=False)
self.js_sub = rospy.Subscriber("~joint_states", numpy_msg(JointState), self.js_cb, queue_size=3, tcp_nodelay=True)
self.js_des_pos = rospy.Subscriber("~pos_command_in", numpy_msg(Float32MultiArray), self.des_pos_cb, queue_size=3, tcp_nodelay=True)
def main(self):
root = Tkinter.Tk()
self.root = root
root.wm_title("Bootstrapping servo interface")
xAchse = pylab.arange(0, 100, 1)
yAchse = pylab.array([0] * 100)
fig = pylab.figure(1)
ax = fig.add_subplot(111)
self.ax = ax
ax.grid(True)
ax.set_title("")
ax.set_xlabel("sensel")
ax.set_ylabel("")
ax.axis([0, 100, -1.5, 1.5])
self.line_y = ax.plot(xAchse, yAchse, 'b.')
self.line_y_goal = ax.plot(xAchse, yAchse, 'r.')
canvas = FigureCanvasTkAgg(fig, master=root)
canvas.show()
canvas.get_tk_widget().pack(side=Tkinter.TOP, fill=Tkinter.BOTH, expand=1)
toolbar = NavigationToolbar2TkAgg(canvas, root)
toolbar.update()
canvas._tkcanvas.pack(side=Tkinter.TOP, fill=Tkinter.BOTH, expand=1)
self.canvas = canvas
self.button = Tkinter.Button(master=root, text='Quit', command=self._quit)
self.button.pack(side=Tkinter.BOTTOM)
self.wScale = Tkinter.Scale(master=root, label="View Width:", from_=3, to=1000, sliderlength=30,
length=ax.get_frame().get_window_extent().width, orient=Tkinter.HORIZONTAL)
self.wScale2 = Tkinter.Scale(master=root, label="Generation Speed:", from_=1, to=200, sliderlength=30,
length=ax.get_frame().get_window_extent().width, orient=Tkinter.HORIZONTAL)
self.wScale2.pack(side=Tkinter.BOTTOM)
self.wScale.pack(side=Tkinter.BOTTOM)
self.wScale.set(100)
self.wScale2.set(self.wScale2['to'] - 10)
self.root.protocol("WM_DELETE_WINDOW", self._quit) # thanks aurelienvlg
self.root.after(500, self.RealtimePloter)
rospy.init_node('gui')
rospy.Subscriber('/servo_manager/y', numpy_msg(Raw), self.callback_arrived, callback_args='y')
rospy.Subscriber('/servo_manager/y_goal', numpy_msg(Raw), self.callback_arrived, callback_args='y_goal')
rospy.Subscriber('/servo_demo/y', numpy_msg(Raw), self.callback_arrived, callback_args='y')
rospy.Subscriber('/servo_demo/y_goal', numpy_msg(Raw), self.callback_arrived, callback_args='y_goal')
# rospy.Subscriber('/servo_manager/u', numpy_msg(Raw), self.callback_arrived, callback_args='u')
#
self.data = {}
Tkinter.mainloop()
def test_class_identity(self):
from rospy.numpy_msg import numpy_msg
self.assert_(isinstance(numpy_msg(Floats)(), numpy_msg(Floats)))
FloatsNP = numpy_msg(Floats)
FloatsNP2 = numpy_msg(Floats)
self.assert_(FloatsNP is FloatsNP2)
def __init__(self, topics, params):
""" Constructor
@param topics: dictionary mapping topic names to publisher handles
valid topic names are {"cloud", "scan0", "scan1", "scan2", "scan3"}
@type topics: dict {topic_name:publisher_handle}
@param params: dictionary mapping parameter names to values.
Where applicable, the parameters must be in device units (e.g. ticks)
@type params: dict {ros_parameter_string:value}
"""
self.params = params
self.topics = topics
# LaserScan messages (numbered 0-3, 0 is the lowest scan plane)
# rewire for numpy serialization on publish
self.scans = [numpy_msg(LaserScan)(),
numpy_msg(LaserScan)(),
numpy_msg(LaserScan)(),
numpy_msg(LaserScan)()]
self._init_scans()
# PointCloud2 message
# rewire for numpy serialization on publish
self.point_cloud = numpy_msg(PointCloud2)()
self._init_point_cloud()
# put variables into the namespace to prevent
# attribute exceptions when the class is abused
self.header = None
self.x = None
self.y = None
self.z = None
self.echo = None
self.layer = None
self.flags = None
self.echo_w = None
self.h_angle = None
self.rads_per_tick = None
self.pc_data = None
self.last_start_time = None
self.rads_per_tick = None
self.seq_num = -1
self.time_delta = rospy.Duration()
self.n_points = 0
# timestamp smoothing
self.smoothtime = None
self.smoothtime_prev = None
self.recv_time_prev = None
self.known_delta_t = rospy.Duration(256.0/self.params['scan_frequency'])
self.time_smoothing_factor = self.params['time_smoothing_factor']
self.time_error_threshold = self.params['time_error_threshold']
self.total_err = 0.0 #integrate errors
self.header = {}.fromkeys(self.header_keys)
def __init__(self):
self._coll_checked = False
self._pub_joints = rospy.Publisher('joint_state_check', numpy_msg(Float32MultiArray), queue_size = 10, latch=True)
self._sub = rospy.Subscriber('collision_check', Int16, self.sub_cb)
# self._pub_path = rospy.Publisher('joint_path',numpy_msg(Float32MultiArray), queue_size = 10)
self._pub_traj = rospy.Publisher('joint_traj',numpy_msg(Float32MultiArray), queue_size = 10)
self._iter = 0
self._executing = True
self._sub_demo_state = rospy.Subscriber('demo_state', Int16, self.sub_demo)
self._sub_pos_state = rospy.Subscriber('pos_state', Int16, self.sub_pos)
self._plot_demo = get_param('plot_demo', False)
rospy.loginfo("Here's my demo state: ")
rospy.loginfo(self._plot_demo)
self._plotIndex = -1
def main(self):
rospy.init_node('rviz_visualizer_node')
self.r = rostopic.ROSTopicHz(-1)
rospy.Subscriber('/reactive_cones',
numpy_msg(Floats),
self.r.callback_hz,
callback_args='/global_map_markers')
rospy.Subscriber('/reactive_cones', numpy_msg(Floats),
self.callback_reactive)
rospy.Subscriber('/global_map_markers', numpy_msg(Floats),
self.callback_global)
rospy.Subscriber('/odometry/filtered', Odometry, self.callback_odom)
rospy.Timer(rospy.Duration(0.1), self.publish_FOV)
rospy.Timer(rospy.Duration(0.1), self.lifetime_global)
rospy.spin()
def ComBordersPublisher(refInf, refSup):
pub = rospy.Publisher('sot_controller/com_borders', numpy_msg(Floats))
a = numpy.concatenate((numpy.array(refInf, dtype=numpy.float32),
numpy.array(refSup, dtype=numpy.float32)))
#r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
pub.publish(a)
def run_node(self):
rospy.init_node('kb_admin', anonymous=False)
rospy.loginfo('Back robot is up and running')
rospy.Subscriber('config_robot_side_receiver', numpy_msg(StringArray),
self.on_data_passing)
self.publish()
rospy.spin()
def __init__(self):
print 'Scouting system for ROS topics and types...'
print 'The more you decide to filter, less time the scouting will take.'
self._scouter = scouter.Scouter()
self._pub = rospy.Publisher('config_robot_side_sender',
numpy_msg(StringArray),
queue_size=10)
def talker():
cmd_publisher= rospy.Publisher('cmd_drive', cmd_drive,queue_size=10)
data_pub = rospy.Publisher('floats', numpy_msg(Floats),queue_size=10)
rospy.Subscriber("/IMU", Odometry, ImuCallback)
rospy.Subscriber("/GPS/fix",NavSatFix,retour_gps)
rospy.init_node('LQR', anonymous=True)
r = rospy.Rate(200) # 10hz
simul_time = rospy.get_param('~simulation_time', '10')
# == Dynamic Model ======================================================
# == Steady State ======================================================
# == LQR control (Gains) ======================================================
C=np.array([ [0, 1,0, 0],
[0,0, 1, 0],
[0,0,0, 1]])
R=np.array([ [10000,0],
[0,10000]])
Q=np.array([ [100, 0,0, 0],
[0,100, 0, 0],
[0,0, 100, 0],
[0,0,0, 100]])
def listener():
global y_offset
global y_offset_list
global net_name
global CLASSES
global N_CLASSES
global N_BINS
rospy.init_node('nms_detections', anonymous=True)
net_name = rospy.get_param('~net', 'regular')
text = '[NMS] Using %s' % (net_name)
rospy.loginfo(text)
y_offset = rospy.get_param('~y_offset', 0)
y_offset_list = [0, y_offset, 0, y_offset]
det_sub = rospy.Subscriber("cnn_detection", numpy_msg(Detection), callback)
configfile = os.path.join(pkg_path, 'models', NETS[net_name][3])
cfg_from_file(configfile)
CLASSES = cfg.CLASSES
N_CLASSES = len(CLASSES)
N_BINS = cfg.VIEWP_BINS
rospy.spin()
def start(self):
if not self._created:
print "Starting subscription ..."
rospy.Subscriber("SoundData_1", numpy_msg(Float64MultiArray),
self.callback)
print "Done."
self._created = True
def image_callback(msg):
pub = rospy.Publisher('2D_coordinates', numpy_msg(Floats),queue_size=10)
# Convert your ROS Image message to OpenCV2
cv2_img = bridge.imgmsg_to_cv2(msg, "bgr8")
gray = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2GRAY)
faces = face_cascade.detectMultiScale(
gray,
scaleFactor=2,
minNeighbors=5,
minSize=(30, 30),
flags=cv2.CASCADE_SCALE_IMAGE
)
for (x, y, w, h) in faces:
rect = cv2.rectangle(cv2_img, (x, y), (x+w, y+h), (0, 0, 255), 2)
cv2.line(rect,(x+w/2,y),(x+w/2,y+h),(255,255,255),1)
cv2.line(cv2_img,(320,0),(320,480),(255,255,255),3)
a = numpy.array([x+w/2, y+h/2], dtype=numpy.float32)
print('inter connecting..........')
print(len(faces))
cv2.imshow("pic3", cv2_img)
cv2.waitKey(3)
pub.publish(a)
def __init__(self):
print('starting [DRIVER] node')
init_model()
Driver.controls_pub = rospy.Publisher('controls',
BDDMsg.BDDControlsMsg,
queue_size=None)
rospy.Subscriber('/zed/left/image_rect_color',
numpy_msg(Image),
self.left_callback,
queue_size=5)
rospy.Subscriber('/zed/right/image_rect_color',
numpy_msg(Image),
self.right_callback,
queue_size=5)
def talker(self):
pubMatrixB = rospy.Publisher('floatsBToMaster',
numpy_msg(Floats),
queue_size=10,
latch=True)
pubMatrixBRow = rospy.Publisher('rowBToMaster',
Int16,
queue_size=9,
latch=True)
pubMatrixBCol = rospy.Publisher('colBToMaster',
Int16,
queue_size=8,
latch=True)
print "Talker reached \n"
rate = rospy.Rate(1) # 10hz
connectionsReady = False
while not connectionsReady:
connectionMatrixB = pubMatrixB.get_num_connections()
connectionMatrixBRow = pubMatrixBRow.get_num_connections()
connectionMatrixBCol = pubMatrixBCol.get_num_connections()
if connectionMatrixB > 0 and connectionMatrixBRow > 0 and connectionMatrixBCol > 0:
pubMatrixB.publish(self.matrix.data)
pubMatrixBRow.publish(self.matrixRow.data)
pubMatrixBCol.publish(self.matrixCol.data)
print "Messages to Master published. \n"
connectionsReady = True
else:
rate.sleep()
def main(stdscr):
global cmd
rospy.init_node('keyboard_input', anonymous=True)
pub = rospy.Publisher('kobuki/cmd_vel', numpy_msg(Twist))
rate = rospy.Rate(50)
stdscr.nodelay(True)
key=""
stdscr.clear()
stdscr.scrollok(1)
stdscr.addstr("Menu:\n")
stdscr.addstr("Forward: KEY_UP \n")
stdscr.addstr("Backward: KEY_DOWN \n")
stdscr.addstr("Left: KEY_LEFT \n")
stdscr.addstr("Right: KEY_RIGHT \n")
stdscr.addstr("Stop: SPACE BAR \n")
stdscr.addstr("Quit: Q \n")
while not rospy.is_shutdown():
try:
key = str(stdscr.getkey())
processKey(stdscr, key)
# print cmd
if key == '\n':
break
except Exception as e:
# No input
pass
pub.publish(cmd)
rate.sleep()
def run_node(self):
rospy.init_node('back_planner_side', anonymous=True)
rospy.loginfo('Back planner is up and running')
rospy.Subscriber('config_plan_side_receiver', numpy_msg(StringArray),
self.on_data_passing)
self.publish_data2edit()
rospy.spin()
def listener():
rospy.init_node('listener_sensor1')
rospy.Subscriber('wtsft_sensor1', numpy_msg(Floats), callback)
#ani = animation.FuncAnimation(fig,callback,frames = 400,interval=10 ,blit =True)
plt.show()
rospy.spin()
def __init__(self):
#a publisher for our line marker
self.line_pub = rospy.Publisher(self.WALL_TOPIC, Marker, queue_size=1)
#a subscriber to get the laserscan data
rospy.Subscriber(self.SCAN_TOPIC, numpy_msg(LaserScan),
self.laser_callback)
def __init__(self):
rospy.init_node('findbox', anonymous=True)
self.physical_robot = False
if self.physical_robot:
rospy.Subscriber("/scan/long_range",sensor_msgs.msg.LaserScan,self.cb_scan, queue_size=1)
else:
rospy.Subscriber("/scan",sensor_msgs.msg.LaserScan,self.cb_scan, queue_size=1)
rospy.Subscriber('/odometry/filtered',Odometry, self.cb_odom)
self.bearing_pub = rospy.Publisher("/detection",numpy_msg(Floats), queue_size=1)
self.bridge = CvBridge()
self.dist_min = 0.25
self.dist_max = 2.0 # 1m is real target, 41in for height
self.ylen_lim = 2
self.ang_min = -1.57
self.ang_max = 1.57
self.R = None
self.max_range = 60
self.arena_xpos = 100 #rospy.get_param('arena_xpos')
self.arena_xneg = -100 #rospy.get_param('arena_xneg')
self.arena_ypos = -100 #rospy.get_param('arena_ypos')
self.arena_yneg = 100 #rospy.get_param('arena_yneg')
self.rate = rospy.Rate(10)
self.scan_dist_thresh = 15.0 # Distance threshold to split obj into 2 obj.
self.plot_data = False
self.image_output = rospy.Publisher("/output/keyevent_image",Image,
queue_size=1)
def __init__(self, sink):
self.text_pub = rospy.Publisher(sink,
numpy_msg(Floats),
queue_size=10,
latch=True)
self.callback()
def __init__(self):
# self.image_pub = rospy.Publisher("image_topic_2",Image)
self.CMT = CMT.CMT()
self.bridge = CvBridge()
self.first = True
self.rect = None
self.init_img = None
self.center = None
self.prev_center = None
self.h = None
self.w = None
self.linear_speed_x = 0.1
self.k_yaw = 0.0005
self.k_alt = 0.0005
self.image_sub = rospy.Subscriber("/rexrov/rexrov/camera/camera_image",
Image, self.callback)
self.des_vel_pub = rospy.Publisher("/rexrov/cmd_vel",
numpy_msg(geometry_msgs.Twist),
queue_size=1)
def step(self, action):
"""
Parameters
----------
action : [change in x, change in y, change in z]
Returns
-------
ob, reward, episode_over, info : tuple
ob (object) :
either current position or an observation object, depending on
the type of environment this is representing
reward (float) :
negative, squared, l2 distance between current position and
goal position
episode_over (bool) :
Whether or not we have reached the goal
info (dict) :
For now, all this does is keep track of the total distance from goal.
This is used for rlkit to get the final total distance after evaluation.
See function get_diagnostics for more info.
"""
action = np.array(action, dtype=np.float32)
# ROS specific here
self.action_publisher.publish(action)
self.current_pos = np.array(
rospy.wait_for_message("/replab/action/observation",
numpy_msg(Floats)).data)
return self._generate_step_tuple()
def __init__(self):
self.image_sub = rospy.Subscriber("/rexrov/rexrov/camera/camera_image",
Image, self.image_callback)
self.des_vel_pub = rospy.Publisher("/rexrov/cmd_vel",
numpy_msg(geometry_msgs.Twist),
queue_size=1)
self.pose_sub = rospy.Subscriber("/rexrov/pose_gt", Odometry,
self.pose_callback)
self.model_state_pub = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=1)
self.jerk_sub = rospy.Subscriber("/jerk", Float32, self.jerk_callback)
self.position = np.array([0., 0., 0.])
self.image = None
self.linear_speed = 0.3
self.bridge = CvBridge()
self.reward = None
self.terminal = None
self.state = None
self.hit = False
self.state_dim = 80 * 80
self.action_dim = 2
self.action_bound = 0.5
self.now = time.time()
rospy.init_node('sub_env', anonymous=True)
def __init__(self):
rospy.init_node(NODE_NAME)
sys.path.append(
'~/Documents/RobotSemantics/semseg_ws/src/image_segnet/object_class_pcl/src'
)
for name, default in PARAM_LIST:
setattr(self, name, rospy.get_param(PARAM_PREFIX + name, default))
rospy.Subscriber(self.in_pcl, PointCloud2, self.pcl_cb)
rospy.Subscriber(self.in_viz + '/' + self.camera_used + '/camera_info',
CameraInfo, self.ci_cb)
rospy.Subscriber(
self.in_viz + '/' + self.camera_used + '/image/compressed',
CompressedImage, self.image_cb)
rospy.Subscriber(self.in_semseg + '/class_matrix', numpy_msg(Floats),
self.clmat_cb)
self.pcl_pub = rospy.Publisher(self.out_pcl,
PointCloud2,
queue_size=20)
self.clmats = {}
self.cis = {}
self.imgsC = {}
self.timestamps = []
self.counterTS = 0
self.counter = 0
#SHOULD BE CHANGED TO SOMETHING ADAPTIVE - NOW manually SETTED
self.camera0PM = [
638.81494, 0, 625.98561, 0, 0, 585.79797, 748.57858, 0, 0, 0, 1, 0
]
self.tf_listener = tf.TransformListener()
self.adjustment = np.dot(
tf_help.rotation_matrix(math.pi / 2, (0, 1, 0)),
tf_help.rotation_matrix(math.pi, (0, 0, 1)))
def __init__(self):
self.status = 'STOPPED'
RG.setmode(RG.BCM)
RG.setup(14, RG.OUT)
self.AnglesDict = {}
self.Delta = 1.
self.angles_off, self.direction, self.angles_check, self.legs_motors_ids = parsser(
'/home/pi/Robot_Control/angles.txt')
rospy.init_node('angle_ctrl')
self.publisher_status = rospy.Publisher('status', String, queue_size=1)
self.publisher_legs = rospy.Publisher("/dxl/command_position",
CommandPosition,
queue_size=1)
self.enabler = rospy.Publisher("/dxl/enable", Bool, queue_size=1)
self.enabler.publish(True)
self.subscriber_status = rospy.Subscriber('status', String,
self.update_status)
self.subscriber_status = rospy.Subscriber('led', Int8,
self.SwitchLight)
self.subscriber_delta = rospy.Subscriber('delta', Float32,
self.UpdateDelta)
rospy.Subscriber("/dxl/chain_state", ChainState, self.UpdateAngles)
[
rospy.Subscriber("angles_raw_leg_{0}".format(i), numpy_msg(Floats),
self.check_angles, i) for i in range(6)
]
print "Initialization done. Running..."
rospy.spin()
def __init__(self):
self.image_pub = rospy.Publisher("contoured_image", Image, queue_size=10)
self.lower = np.array([23,87,255], dtype = "uint8")
self.upper = np.array([63,137,255], dtype = "uint8")
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/stereo/right/image_rect_color", Image, self.callback)
self.image_thresholds = rospy.Subscriber("/threshold_values", numpy_msg(Floats), self.getThresholds)
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
pub = rospy.Publisher('/firefly_2/fov_adj', numpy_msg(Floats),queue_size=10)
rospy.init_node('fov2', anonymous=True)
#Subscribing to Experimental Topics
rospy.Subscriber("/firefly_1/ground_truth/pose", Pose, pos_2)
rospy.Subscriber("/firefly_3/ground_truth/pose", Pose, pos_3)
rospy.Subscriber("/firefly_4/ground_truth/pose", Pose, pos_4)
rospy.Subscriber("/firefly_2/ground_truth/pose", Pose, fov)
#########################################################################################
#############################################################################################
rate = rospy.Rate(10) # 10hz, may need this to be 100HZ
while not rospy.is_shutdown():
#a = np.array([2.10,3.10,2.14,2.34,2.34,3.14],dtype=np.float32)
pub.publish(adj)
rate.sleep()
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def __init__(self,name="slm_laser", udp_addr="128.2.0.201", port=30, speed=10, frame="/slm", vis_spot=False, lo_res=False ):
self.udp_addr=udp_addr
self.udp_port=port
#ros
rospy.init_node(name)
self.pub=rospy.Publisher(name, numpy_msg(LaserScan))
self.ls.range_max=655
self.ls.range_min=0.01
self.ls.header.frame_id=frame
self.seq=0
self.comms_fail=0
#Open UDP port
self.sock = socket.socket( socket.AF_INET, # Internet
socket.SOCK_DGRAM ) # UDP
self.sock.settimeout(0.1)
self.set_enable_reply(True)
self.send_cmd("X1\n") # Set output to Binary
self.set_speed(speed)
self.set_visible(vis_spot)
self.set_resolution(lo_res)
self.send_cmd("S\n") # Start motor spinning
self.set_laser_on(True)
def __init__(self):
''' Iniate self and subscribe to /scan topic '''
# which nexus?
self.name = 'n_2'
# Desired distance - used for sending if no z is found or if the dataprocessingnode is shutdown:
# robot not influenced if one z not found
self.d = np.float32(0.8)
self.dd = np.float32(np.sqrt(np.square(self.d) + np.square(self.d)))
# set min and max values for filtering ranges in meter during initiation
self.min_range = 0.25
self.max_range = 1.2
# set variables for tracking
# 0: init, 1: tracking, 2:
self.k = 0
# prepare shutdown
self.running = True
rospy.on_shutdown(self.shutdown)
# prepare publisher
self.pub = rospy.Publisher(
self.name + '/z_values', numpy_msg(Floats),
queue_size=1) #Publish the distances and angles to agents in range
self.PUB = rospy.Publisher(
self.name + '/agents', Int32,
queue_size=1) #Publish the amount of surrounding agents
# subscribe to /scan topic with calculate_z as calback
#rospy.Subscriber('/nexus1/scan', LaserScan, self.calculate_z) #############3
rospy.Subscriber('/n_2hokuyo_points', LaserScan, self.calculate_z)
np.set_printoptions(precision=2)
def ref_generation():
xbox_ctrl = rospy.Publisher('xboxctrl', numpy_msg(Floats))
rospy.init_node('xboxbridge', anonymous=True)
rate = rospy.Rate(100) # Run no faster than 100hz
UDP_IP = "192.168.10.81"
UDP_PORT = 27300
sock = socket.socket(socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
sock.bind((UDP_IP, UDP_PORT))
sock.setblocking(0)
packer = struct.Struct('c c c c f f f f B B h')
while not rospy.is_shutdown():
try:
data = sock.recv(48) # buffer size is 48 bytes
if len(data) == 24:
if data[0] == "X" and data[1] == "B" and data[2] == "O" and data[3] == "X":
output = packer.unpack(data)
ctrl = numpy.array([output[4],output[5],output[6],output[7],output[8]], dtype=numpy.float32)
xbox_ctrl.publish(ctrl)
#print output
except:
# print "Nothing"
if str(sys.exc_info()[1]) != '[Errno 11] Resource temporarily unavailable':
rospy.logwarn("Unexpected error: %s", sys.exc_info()[1])
nothing = 0
rate.sleep()
def __init__(self):
# Data options (change me)
self.im_height = 720
self.im_width = 1280
self.tcp_host_ip = '127.0.0.1'
#self.tcp_host_ip = '192.168.0.5'
self.tcp_port = 50000
self.buffer_size = 4098 # 4 KiB
self.color_img = np.empty((self.im_height, self.im_width, 3))
self.depth_img = np.empty((self.im_height, self.im_width))
# Connect to server
self.tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.tcp_socket.connect((self.tcp_host_ip, self.tcp_port))
# ROS Interfacing
self.bridge = CvBridge()
self.image_pub = rospy.Publisher("image_topic", Image, queue_size=10)
self.np_pub = rospy.Publisher('np_data',
numpy_msg(Float32),
queue_size=10)
_thread = threading.Thread(target=self.imageCB)
_thread.daemon = True
_thread.start()
# Connect to server
self.intrinsics = None
self.get_data()
self.get_data_flg = False
def listener():
rospy.init_node('srp_phat_fd_gui', anonymous=True)
rospy.Subscriber('/srp_phat_fd_value',
numpy_msg(Float32MultiArray),
callback,
queue_size=1)
rospy.spin()
def __init__(self):
# initialize potential field variables
self.charge_laser_particle = 0.055
self.charge_forward_boost = 25.0
self.boost_distance = 0.5
self.certainty = 0
self.p_speed = 0.05
self.p_steering = 1.0
self.p_d = 0.5
self.turn_right = False
self.last_y = 0.0
self.turn_x_component = np.zeros(1)
self.turn_y_component = np.zeros(1)
# subscribe to turning signal
rospy.Subscriber("/turnRight", Bool, self.bool_back)
# subscribe to laserscans. Force output message data to be in numpy arrays.
rospy.Subscriber("/scan", numpy_msg(LaserScan), self.scan_callback)
self.angle_min = 0
self.angle_increment = 0
# output a pose of where we want to go
self.pub_goal = rospy.Publisher("~potentialFieldGoal",
PointStamped,
queue_size=1)
self.pub_nav = rospy.Publisher(
"/vesc/ackermann_cmd_mux/input/navigation",
AckermannDriveStamped,
queue_size=1)
self.speedHist = 1.0
def stream(kinectTopic):
rate = rospy.Rate(10)
depthSub = rospy.Subscriber(kinectTopic, PointCloud2, callback)
rospy.Subscriber("vec", numpy_msg(vec), callbackVec)
print("Starting grasp procedure. Press Ctrl-C to stop...")
while not rospy.is_shutdown():
rate.sleep()
def main(args):
#global x,y
rospy.init_node('objectGrasp', anonymous=True)
rospy.Subscriber("vec", numpy_msg(vec), callback)
#print x,y
rospy.spin()
return 0
def __init__(self):
self.node = rospy.init_node('qav250', anonymous=True)
# publisher for RC commands
self.rc_pub = rospy.Publisher('rcctrl', numpy_msg(Floats))
# subscriber for Vicon data
self.vicon_sub = rospy.Subscriber('drone', TransformStamped, self.vicon_callback)
# subscriber for reference point input
self.point_sub = rospy.Subscriber('refpoint', Point, self.ref_callback)
# subscriber for integrator freeze
self.freeze_int = rospy.Subscriber('freeze_int', Bool, self.freeze_int_callback)
# PID controller for each axis
self.pitch_pid = rospidlib.Rospid(0.075,0.0,0.11,'~pitch') # pitch
self.roll_pid = rospidlib.Rospid(0.075,0.0,0.11,'~roll') # roll
self.thrust_pid = rospidlib.Rospid(0.2,0.03,0.15,'~thrust') # height
self.yaw_pid = rospidlib.Rospid(0.25,0.0,0.0,'~yaw') # yaw
# freeze the thrust integrator until flying
self.thrust_pid.freeze_integrator()
# point reference input
self.ref_point = Point()
# default is at origin, 1.8m off ground
self.ref_point.z = rospy.get_param('init_z', 0.8)
self.ref_point.x = rospy.get_param('init_x', 0.0)
self.ref_point.y = rospy.get_param('init_y', 0.0)
# get min/max points
self.min_x = rospy.get_param('/min_x', -5.0)
self.min_y = rospy.get_param('/min_y', -5.0)
self.min_z = rospy.get_param('/min_z', 0.0)
self.max_x = rospy.get_param('/max_x', 5.0)
self.max_y = rospy.get_param('/max_y', 5.0)
self.max_z = rospy.get_param('/max_z', 5.0)
self.rel_threshold = rospy.get_param('/rel_threshold', 2.5)
#broadcast target point to RViz
self.target_br = tf.TransformBroadcaster()
def __init__(self):
rospy.init_node('findbox', anonymous=True)
self.physical_robot = False
if self.physical_robot:
rospy.Subscriber("/scan/long_range",sensor_msgs.msg.LaserScan,self.cb_scan, queue_size=1)
else:
rospy.Subscriber("/scan",sensor_msgs.msg.LaserScan,self.cb_scan, queue_size=1)
rospy.Subscriber('/odometry/filtered',Odometry, self.cb_odom)
self.bearing_pub = rospy.Publisher("/detection",numpy_msg(Floats), queue_size=1)
self.bridge = CvBridge()
self.dist_min = 0.75
self.dist_max = 1.5 # 1m is real target, 41in for height
self.ylen_lim = 4
self.ang_min = -1.57
self.ang_max = 1.57
self.R = None
self.arena_xpos = 100 #rospy.get_param('arena_xpos')
self.arena_xneg = -100 #rospy.get_param('arena_xneg')
self.arena_ypos = -100 #rospy.get_param('arena_ypos')
self.arena_yneg = 100 #rospy.get_param('arena_yneg')
self.rate = rospy.Rate(10)
self.scan_dist_thresh = 5.0 # Distance threshold to split obj into 2 obj.
self.plot_data = True
self.image_output = rospy.Publisher("/output/keyevent_image",Image,
queue_size=1)
def test_floats(self):
if disable:
return
vals = [1.0, 2.1, 3.2, 4.3, 5.4, 6.5]
b = cStringIO.StringIO()
f = Floats(
numpy.array([1.0, 2.1, 3.2, 4.3, 5.4, 6.5], dtype=numpy.float32))
f.serialize(b)
# deserialize twice, once with numpy wrappers, once without
f2 = Floats()
self.assert_(type(f2.data) == list)
f2.deserialize(b.getvalue())
for x, y in zip(f2.data, vals):
self.assertAlmostEquals(x, y, 2)
from rospy.numpy_msg import numpy_msg
f3 = numpy_msg(Floats)()
if 0:
# future
self.assert_(isinstance(f3.data, numpy.ndarray), type(f3.data))
f3.deserialize(b.getvalue())
self.assert_(isinstance(f3.data, numpy.ndarray), type(f3.data))
v = numpy.equal(
f3.data,
numpy.array([1.0, 2.1, 3.2, 4.3, 5.4, 6.5], dtype=numpy.float32))
self.assert_(v.all())
def __init__(self):
print('AccelerationControllerNode: initializing node')
self.vehicle_model = Vehicle()
self.ready = False
self.mass = 1.
self.inertial_tensor = numpy.identity(3)
self.mass_inertial_matrix = numpy.zeros((6, 6))
self.vel_ = numpy.zeros(6)
self.acc_ = numpy.zeros(6)
# ROS infrastructure
self.pub_gen_force = rospy.Publisher('thruster_manager/input',
Wrench,
queue_size=1)
self.sub_accel = rospy.Subscriber('cmd_accel', numpy_msg(Accel),
self.accel_callback)
self.sub_vel = rospy.Subscriber('/anahita/pose_gt',
numpy_msg(Odometry), self.vel_callback)
self.sub_force = rospy.Subscriber('cmd_force', numpy_msg(Accel),
self.force_callback)
"""self.pub_gen_force = rospy.Publisher(
'thruster_manager/input', Wrench, queue_size=1)"""
if not rospy.has_param("pid/mass"):
raise rospy.ROSException("UUV's mass was not provided")
if not rospy.has_param("pid/inertial"):
raise rospy.ROSException("UUV's inertial was not provided")
self.mass = rospy.get_param("pid/mass")
self.inertial = rospy.get_param("pid/inertial")
# update mass, moments of inertia
self.inertial_tensor = numpy.array([
[self.inertial['ixx'], self.inertial['ixy'], self.inertial['ixz']],
[self.inertial['ixy'], self.inertial['iyy'], self.inertial['iyz']],
[self.inertial['ixz'], self.inertial['iyz'], self.inertial['izz']]
])
self.mass_inertial_matrix = numpy.vstack(
(numpy.hstack((self.mass * numpy.identity(3), numpy.zeros(
(3, 3)))),
numpy.hstack((numpy.zeros((3, 3)), self.inertial_tensor))))
print self.mass_inertial_matrix
self.ready = True
def __init__(self):
super(KeyboardController,self).__init__()
self.pitch = 0
self.roll = 0
self.yaw_velocity = 0
self.z_velocity = 0
# Subscribe to visp auto tracker
self.vispStatusSub = rospy.Subscriber('/visp_auto_tracker/status',numpy_msg(Int8),self.ReceiveVispStatus)
self.vispPositionSub = rospy.Subscriber('/visp_auto_tracker/object_position',numpy_msg(PoseStamped),self.ReceiveVispPosition)
# Variables for visp auto tracker
self.vispStatus = 0
self.vispPosition = None
self.vispStatusActioned = 0
def talker():
pub = rospy.Publisher('floats', numpy_msg(Floats),queue_size=10)
rospy.init_node('talker', anonymous=True)
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
a = numpy.array([1.0, 2.1, 3.2, 4.3, 5.4, 6.5], dtype=numpy.float32)
pub.publish(a)
r.sleep()
def __init__(self):
print('VelocityControllerNode: initializing node')
self.config = {}
self.v_linear_des = numpy.zeros(3)
self.v_angular_des = numpy.zeros(3)
# Initialize pids with default parameters
self.pid_angular = PIDRegulator(1, 0, 0, 1)
self.pid_linear = PIDRegulator(1, 0, 0, 1)
# ROS infrastructure
self.sub_cmd_vel = rospy.Subscriber('cmd_vel', numpy_msg(geometry_msgs.Twist), self.cmd_vel_callback)
self.sub_odometry = rospy.Subscriber('odom', numpy_msg(Odometry), self.odometry_callback)
self.pub_cmd_accel = rospy.Publisher('cmd_accel', geometry_msgs.Accel, queue_size=10)
self.srv_reconfigure = Server(VelocityControlConfig, self.config_callback)
def __init__(self):
''' Initiate self and subscribe to /n_1hokuyo_points topic '''
# which nexus?
self.name = 'n_3'
# Desired distance - used for sending if no z is found or if the dataprocessingnode is shutdown:
# robot not influenced if one z not found
self.d = np.float32(0.8)
self.dd = np.float32(np.sqrt(np.square(self.d) + np.square(self.d)))
# NOTE: not used anymore
# set min and max values for filtering ranges. Will change during tracking
self.min_range = 0.35
self.max_range = 1.5
# state of processing
# 0: init, 1: tracking, 2:
self.k = 0
# the robots
self.robots = []
# the obstacles
self.obstacles = []
# prepare shutdown
self.running = True
rospy.on_shutdown(self.shutdown)
# prepare publisher
self.pub_r = rospy.Publisher(self.name + '/r_values',
numpy_msg(Floats),
queue_size=1)
self.pub_obs = rospy.Publisher(self.name + '/obstacles',
numpy_msg(Floats),
queue_size=1)
# prepare publisher for modified laser scan data
self.pub_scan = rospy.Publisher('/n_3scan', LaserScan, queue_size=50)
self.num_readings = 360
self.laser_frequency = 80
# subscribe to /scan topic with calculate_z as calback
rospy.Subscriber('/n_3hokuyo_points', LaserScan, self.calculate_r)
np.set_printoptions(precision=2)
def __init__(self):
self.p = pyaudio.PyAudio()
self.stream = self.p.open(format=self.FORMAT,
channels=self.CHANNELS,
rate=self.FS,
input=True,
frames_per_buffer=self.CHUNK)
self.rec_publisher = rospy.Publisher('/wav',numpy_msg(Floats),queue_size =20)
def talker():
pub = rospy.Publisher('floats', numpy_msg(Floats),queue_size=10)
rospy.init_node('talker', anonymous=True)
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
a = numpy.array([1.0, 2.1, 3.2, 4.3, 5.4, 6.5], dtype=numpy.float32)
pub.publish(a)
r.sleep()
def __init__(self):
self.frame = None
self.colors = []
self.hasNewFrame = False
self.thresholds = np.array([0,0,0,0,0,0], dtype= np.float32)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/stereo/right/image_raw", Image, self.callback)
self.image_thresholds = rospy.Publisher("/threshold_values",numpy_msg(Floats), queue_size=10)
client = dynamic_reconfigure.client.Client("vision_server", timeout=30, config_callback=callback)
def __init__(self):
print('PositionControllerNode: initializing node')
self.config = {}
self.pos_des = numpy.zeros(3)
self.quat_des = numpy.array([0, 0, 0, 1])
self.initialized = False
# Initialize pids with default parameters
self.pid_rot = PIDRegulator(1, 0, 0, 1)
self.pid_pos = PIDRegulator(1, 0, 0, 1)
# ROS infrastructure
self.sub_cmd_pose = rospy.Subscriber('cmd_pose', numpy_msg(geometry_msgs.PoseStamped), self.cmd_pose_callback)
self.sub_odometry = rospy.Subscriber('odom', numpy_msg(Odometry), self.odometry_callback)
self.pub_cmd_vel = rospy.Publisher('cmd_vel', geometry_msgs.Twist, queue_size=10)
self.srv_reconfigure = Server(PositionControlConfig, self.config_callback)
def __init__(self):
self.backend = ModelInterface.load(self.INPUT_MODEL)
try:
fs, signal = read_wav(self.BG)
self.backend.init_noise(fs, signal)
except:
print "file not found!"
self.pub = rospy.Publisher('/speaker',String,queue_size = 10)
self.sub = rospy.Subscriber('/wav',numpy_msg(Floats),self.task_predict)
def __init__(self):
#Initialize ros publisher, subscriber
self.pub1 = rospy.Publisher('prediction',PredictionMSG,queue_size=1)
self.sub1 = rospy.Subscriber('cropped_box_image/numpy',numpy_msg(Floats),self.callback,queue_size=1)
self.pub2 = rospy.Publisher('cropped_box_image/image',Image, queue_size=1)
self.bridge = CvBridge()
self.pt1x = -40.0
self.pt1y = 0.0
self.pt2x = 40.0
self.pt2y = 0.0
rospy.loginfo("Initialized!")
def test_cloud(self):
cloud = rospy.Subscriber("/ldmrs/cloud", numpy_msg(PointCloud2), self.callback)
self.get_msg()
self.init_seq()
self.send_ldmrs_msg()
self.callback_event.wait(self.timeout)
self.assertTrue(self.callback_event.isSet(),
"Timeout waiting for cloud message")
self.callback_event.clear()
cloud.unregister()
self.check_point_cloud()
def __init__(self, camera=None):
if camera is None:
camera = ''
self.image_source = camera + "/image_raw"
self.pub_name = camera + "/image_numpy"
self.bridge = CvBridge()
self.np_image_pub = rospy.Publisher(self.pub_name, numpy_msg(Uint16s))
rospy.init_node('image_converter', anonymous=True)
self.image_sub = rospy.Subscriber(self.image_source,Image,self.image_callback)
def __init__(self):
#Initialize ros publisher, subscriber
self.pub1 = rospy.Publisher('prediction',PredictionMSG,queue_size=1)
self.sub1 = rospy.Subscriber('box_image/numpy',numpy_msg(Floats),self.callback,queue_size=1)
self.pub2 = rospy.Publisher('p_box_image/image',Image, queue_size=1)
self.pub3 = rospy.Publisher('predicted_class', String, queue_size=1)
self.bridge = CvBridge()
self.pt1x = -15.0
self.pt1y = 0.0
self.pt2x = 15.0
self.pt2y = 0.0
rospy.loginfo("Evaluator Initialized!")
def __init__(self, topics):
#
self.bridge = CvBridge()
# Flags
self.need_rgb = True
self.need_depth = True
# RGB Subscribers
sub_rgb = message_filters.Subscriber(topics['rgb'], numpy_msg(Image))
sub_rgb_info = message_filters.Subscriber(topics['rgb_info'], CameraInfo)
ts_rgb = message_filters.TimeSynchronizer([sub_rgb, sub_rgb_info], 10)
ts_rgb.registerCallback(self.rgb_callback)
# Depth Subscribers
sub_depth = message_filters.Subscriber(topics['depth'], numpy_msg(Image))
sub_depth_info = message_filters.Subscriber(topics['depth_info'], CameraInfo)
ts_depth = message_filters.TimeSynchronizer([sub_depth, sub_depth_info], 10)
ts_depth.registerCallback(self.depth_callback)
self.pub_filtered = rospy.Publisher('/camera/rgb/image_color_filtered', numpy_msg(Image))
def __init__(self, limb):
ns = 'robot/limb/' + limb + '/'
self._client = actionlib.SimpleActionClient(
ns + "follow_joint_trajectory",
FollowJointTrajectoryAction,
)
self._goal = FollowJointTrajectoryGoal()
self._goal_time_tolerance = rospy.Time(0.1)
self._goal.goal_time_tolerance = self._goal_time_tolerance
server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0))
if not server_up:
rospy.logerr("Timed out waiting for Joint Trajectory"
" Action Server to connect. Start the action server"
" before running example.")
rospy.signal_shutdown("Timed out waiting for Action Server")
sys.exit(1)
self._sub_path = rospy.Subscriber('joint_path', numpy_msg(Float32MultiArray), self.path_cb)
self._sub_traj = rospy.Subscriber('joint_traj', numpy_msg(Float32MultiArray), self.traj_cb)
self._limb_interface = baxter_interface.limb.Limb('right')
self._q_start = np.array([-0.22281071, -0.36470393, 0.36163597, 1.71920897, -0.82719914,
-1.16889336, -0.90888362])
self.clear(limb)
def __init__(self):
limb = 'left'
cv.NamedWindow("Image window0", 1)
cv.NamedWindow("Image window1", 1)
cv.NamedWindow("Image window2", 1)
cv.NamedWindow("Image window3", 1)
cv.NamedWindow("Image window4", 1)
cv.NamedWindow("Image window5", 1)
self.bridge = CvBridge()
self.head_pub = rospy.Publisher('/sdk/xdisplay', Image, latch=True)
##OPEN BAG FILE FOR TASKS
task_num = rospy.get_param("test_task_number")
pre_path = "/opt/ros/groovy/stacks/baxterjd/tasks/left/"
file_name = "kitting_task"+str(task_num)+".bag"
bagfile = rosbag.Bag(pre_path+file_name,'r')
# Read required information
for topic, msg, time in bagfile.read_messages():
if topic == 'train_hsv_val':
self.hsv_callback(msg)
elif topic == 'train_mean_eigen_list':
self.mean_eigen_callback(msg)
elif topic == 'train_shape_topic':
self.shape_callback(msg)
bagfile.close()
#Publishes the center pixel coordinate of the detected object
self.mean_pub = rospy.Publisher('detected_object_error', numpy_msg(Floats))
self.image_sub = rospy.Subscriber("/cameras/"+limb+"_hand_camera/image",Image,self.callback)
self.hessian_threshold = 10
self.image = Image()
self.fingers_calibrated = False
self.gripper = baxter_interface.Gripper(limb)
open = numpy.asarray(cv2.cv.LoadImage("/opt/ros/groovy/stacks/baxterjd/table_detector/src/calibration_open.jpg")[:,:])
close = numpy.asarray(cv2.cv.LoadImage("/opt/ros/groovy/stacks/baxterjd/table_detector/src/calibration_close.jpg")[:,:])
self.calibration_open = cv2.cvtColor(open,cv2.COLOR_BGR2GRAY)
self.calibration_close = cv2.cvtColor(close,cv2.COLOR_BGR2GRAY)
self.subscribed_topic = False
def __init__(self):
# initialize potential field variables
self.charge_laser_particle = 0.07
self.charge_forward_boost = 25.0
self.boost_distance = 0.5
self.p_speed = 0.007
self.p_steering = 1.0
# subscribe to laserscans. Force output message data to be in numpy arrays.
rospy.Subscriber("/scan", numpy_msg(LaserScan), self.scan_callback)
# output a pose of where we want to go
self.pub_goal = rospy.Publisher("~potentialFieldGoal", PointStamped, queue_size=1)
self.pub_nav = rospy.Publisher("/vesc/ackermann_cmd_mux/input/navigation", AckermannDriveStamped, queue_size=1)
def __init__(self):
# create subscribers, timers, clients, etc.
try:
rospy.wait_for_service("/check_state_validity", timeout=5)
except ROSException:
rospy.logwarn("[check_collisions_node] Done waiting for /check_state_validity service... service not found")
rospy.logwarn("shutting down...")
rospy.signal_shutdown("service unavailable")
except ROSInterruptException:
pass
self.coll_client = rospy.ServiceProxy("check_state_validity", GetStateValidity)
self.js_sub = rospy.Subscriber("joint_state_check", numpy_msg(Float32MultiArray), self.js_cb)
self.js_pub = rospy.Publisher("collision_check", Int16, queue_size = 10)
return
def __init__(self, joint_names):
self.joint_names = joint_names
self.num_dof = len(self.joint_names)
self.init_work_variables()
self.js_sub = rospy.Subscriber("~in_joint_states", numpy_msg(JointState), self.js_cb, queue_size=3, tcp_nodelay=True)
self.out_js_pub = rospy.Publisher("~out_joint_states", numpy_msg(JointState), queue_size=3, tcp_nodelay=True, latch=False)