def test_service(self):
self.assertEquals(0, self.msgs_received)
rospy.wait_for_service('republish_tfs', 2.0)
proxy = rospy.ServiceProxy('republish_tfs', RepublishTFs)
result = proxy(
RepublishTFsRequest(source_frames=['foo', 'bar'],
target_frame='world',
angular_thres=0.1,
trans_thres=0.05,
rate=2,
timeout=rospy.Duration(1.0)))
sub = rospy.Subscriber(result.topic_name, TFArray, self.transform_cb)
rospy.sleep(1.3)
self.assertTrue(any([
topic_tuple[0] == result.topic_name
for topic_tuple in rospy.get_published_topics()
]),
msg=str(rospy.get_published_topics()))
sub.unregister()
self.assertEquals(2, self.msgs_received)
rospy.sleep(2.0)
self.assertFalse(
any([
topic_tuple[0] == result.topic_name
for topic_tuple in rospy.get_published_topics()
]))
def check_connections(publishers, verbose):
allCurrentTopics = rospy.get_published_topics()
if verbose:
for topic in allCurrentTopics:
print topic[0]
print('')
counter = 0
start = time.time()
while counter < len(publishers):
#print('Checking for: {}'.format(publishers[counter]))
counter_2 = 0
while counter_2 < len(publishers[counter]):
if verbose:
print('-----> Checking for: {}'.format(publishers[counter][counter_2]))
counter_3 = 0
while not(any(publishers[counter][counter_2] in sublist for sublist in allCurrentTopics)):
allCurrentTopics = rospy.get_published_topics()
if verbose:
print('--> FOUND\n')
counter_2 += 1
counter += 1
end = time.time()
print('Total time to create all publishers: {} seconds'.format(end- start))
return end - start
def talker():
rospy.get_published_topics()
while not rospy.is_shutdown():
hello_str = "hello world %s" % rospy.get_time()
rospy.loginfo(hello_str)
pub.publish(hello_str)
rate.sleep()
def refresh_topics_and_actions(namespace_ros, server, topicsdict, actionsdict,
idx_topics, idx_actions, topics, actions):
ros_topics = rospy.get_published_topics(namespace_ros)
rospy.logdebug(str(ros_topics))
rospy.logdebug(str(rospy.get_published_topics('/move_base_simple')))
for topic_name, topic_type in ros_topics:
if topic_name not in topicsdict or topicsdict[topic_name] is None:
splits = topic_name.split('/')
if "cancel" in splits[-1] or "result" in splits[
-1] or "feedback" in splits[-1] or "goal" in splits[
-1] or "status" in splits[-1]:
rospy.logdebug("Found an action: " + str(topic_name))
correct_name = ros_actions.get_correct_name(topic_name)
if correct_name not in actionsdict:
try:
rospy.loginfo("Creating Action with name: " +
correct_name)
actionsdict[correct_name] = ros_actions.OpcUaROSAction(
server, actions, idx_actions, correct_name,
get_goal_type(correct_name),
get_feedback_type(correct_name))
except (ValueError, TypeError, AttributeError) as e:
print(e)
rospy.logerr(
"Error while creating Action Objects for Action " +
topic_name)
else:
# rospy.loginfo("Ignoring normal topics for debugging...")
topic = OpcUaROSTopic(server, topics, idx_topics, topic_name,
topic_type)
topicsdict[topic_name] = topic
elif numberofsubscribers(
topic_name, topicsdict) <= 1 and "rosout" not in topic_name:
topicsdict[topic_name].recursive_delete_items(
server.server.get_node(ua.NodeId(topic_name, idx_topics)))
del topicsdict[topic_name]
ros_server.own_rosnode_cleanup()
ros_topics = rospy.get_published_topics(namespace_ros)
# use to not get dict changed during iteration errors
tobedeleted = []
for topic_nameOPC in topicsdict:
found = False
for topicROS, topic_type in ros_topics:
if topic_nameOPC == topicROS:
found = True
if not found:
topicsdict[topic_nameOPC].recursive_delete_items(
server.get_node(ua.NodeId(topic_nameOPC, idx_topics)))
tobedeleted.append(topic_nameOPC)
for name in tobedeleted:
del topicsdict[name]
ros_actions.refresh_dict(namespace_ros, actionsdict, topicsdict, server,
idx_actions)
def test_run(self):
print()
timeout_t = rospy.get_time() + 30.0
while not rospy.is_shutdown() and rospy.get_time() < timeout_t:
rospy.sleep(0.1)
topics = rospy.get_published_topics()
if len(topics) > 0:
print("TOPICS: ",
"\n".join(map(str, rospy.get_published_topics())))
return
self.fail("No topics found")
def drive(self):
self.ipub = rospy.Publisher("/ir/image_raw", Image, queue_size=1)
self.dpub = rospy.Publisher("/depth/image_raw", Image, queue_size=1)
self.dsvc = CameraInfoServiceImpl("depth")
self.isvc = CameraInfoServiceImpl("ir")
print rospy.get_published_topics()
try:
self.frameLoop()
except KeyboardInterrupt:
logger("goodbye")
def OccupancyGridToMat(self, Occ):
width = Occ.info.width
height = Occ.info.height
rospy.get_published_topics()
local_map = np.zeros((width, height), dtype="uint8")
for i in range(0, len(Occ.data)):
if (Occ.data[i] == -1):
value = 100
else:
value = Occ.data[i]
local_map[i % width, int(i / width)] = 255 - value * 255.0
return local_map
def __init__(self, agent_type, verbose, threads, supernamespace="PYGZ_"):
try:
rospy.get_published_topics(namespace='/')
except Exception as e:
self.roscore = _popen(['rosmaster', '--core'], verbose, sleeptime=3)
rospy.set_param('/use_sim_time', True)
rospy.init_node('SIMNODE_' + str(uuid.uuid4().get_hex().upper()), anonymous=True, disable_signals=True)
self.world = World(agent_type, 11345 + np.random.randint(0, 9999), verbose, supernamespace)
self.world.run(pause_process=False)
atexit.register(self.kill)
def talker():
pub = rospy.Publisher('owr/control/availableFeeds', activeCameras, queue_size=10)
rospy.init_node('camera_feeds', anonymous=True)
rate = rospy.Rate(1) # 1hz
while not rospy.is_shutdown():
cameraList = activeCameras(); # The array of stream structs
cameraList.num = 0;
index = 0; # Index for the array of streams, used to contiguously fill cameraList
proc = subprocess.Popen(["ls", "/dev/"], stdout=subprocess.PIPE)
(out, err) = proc.communicate()
a = out.split() # Get all directory strings inside /dev/
for word in a:
if re.search("video", word): # Check if word is a video device
nums = re.findall(r'\d+', word)
camNum = int(nums[0]) # Return the number of the video device
topics = rospy.get_published_topics()
topics = numpy.array(topics)
# Fill out the information for the stream
s = stream()
s.stream = camNum
# Check if currently streaming (involves checking current topics
s.on = online( topics, 0, "/cam" + str(camNum))
cameraList.cameras.append(s)
# Increment the num of cameras in the array
cameraList.num += 1
index += 1
camNum = int(7) # Return the number of the video device
topics = rospy.get_published_topics()
topics = numpy.array(topics)
# Fill out the information for the stream
s = stream()
s.stream = camNum
# Check if currently streaming (involves checking current topics
s.on = online( topics, 0, "/cam" + str(camNum))
cameraList.cameras.append(s)
# Increment the num of cameras in the array
cameraList.num += 1
index += 1
rospy.loginfo(cameraList)
pub.publish(cameraList)
rate.sleep()
def update_subscribers(self):
# Look at all of the topics that are being published in the namespace
for topic, topic_type in rospy.get_published_topics(self.namespace):
topic_type = resolve_message_type(topic_type)
# Ignore topics we are already listening to
if topic in self.subscribers:
continue
variable = topic.split('/')[-1]
# Ignore actuator commands
if variable.endswith('_cmd'):
continue
# Identify topics for set points
is_desired = False
if variable.startswith('desired_'):
variable = variable[8:]
is_desired = True
# Ignore topics that aren't named after valid variables
if not variable in self.valid_variables:
continue
# Subscribe to the topics
def callback(item, variable=variable, is_desired=is_desired):
return self.on_data(item, variable, is_desired)
self.subscribers[topic] = rospy.Subscriber(
topic, topic_type, callback
)
def command():
topics = rospy.get_published_topics()
if HUSKY_TOPIC not in topics:
# launch the relevant ros topics
subprocess.Popen(["roslaunch", "husky_control", "control.launch"])
# publish to the 'cmd_vel' ROS topic
p = rospy.Publisher(HUSKY_TOPIC, Twist, queue_size=100)
rospy.sleep(TIMEOUT)
# create a twist message, and set up the linear and angular
# velocity arguments
twist = Twist()
twist.linear.x = LINEAR_VELOCITY
twist.angular.z = ANGULAR_VELOCITY
# announce move, and publish the message
rospy.loginfo("In Motion!")
# Publish Rate - Husky moves for MOVE_TIME * SLEEP_RATE seconds
for i in range(MOVE_TIME):
p.publish(twist)
rospy.sleep(SLEEP_RATE)
# create a new message
twist = Twist()
rospy.loginfo("Stopping now!")
p.publish(twist)
def refresh(self):
self.clear()
topic_list = rospy.get_published_topics()
for topic_path, topic_type in topic_list:
topic_name = topic_path.strip('/')
message_instance = roslib.message.get_message_class(topic_type)()
self.add_message(message_instance, topic_name, topic_type, topic_path)
def get_published_topics(self): """This function returns a list with the current published topics """ topics = [] for topic in rospy.get_published_topics(): topics.append(topic[0]) return topics
def on_record(self, e):
'''Handles record button events'''
if self.recorder:
self.recorder.send_signal(subprocess.signal.SIGINT)
self.recorder = None
wildcard = "Rosbag file (*.bag)|*.bag"
file_dialog = wx.FileDialog(self, message="Save a rosbag file",
wildcard=wildcard, style=wx.SAVE | wx.CHANGE_DIR)
if file_dialog.ShowModal() == wx.ID_OK:
self.recording_path = file_dialog.GetPath()
file_dialog.Destroy()
self.recording_topics = sorted(set(x[0] for x in rospy.get_published_topics()))
self.recording_topics.insert(0, 'All topics')
multi_dialog = wx.MultiChoiceDialog(self, "Pick topics for the rosbag",
"Choose topics", self.recording_topics)
if multi_dialog.ShowModal() == wx.ID_OK:
selections = multi_dialog.GetSelections()
if selections:
if 0 in selections:
self.selected_topics = self.recording_topics[1:]
else:
self.selected_topics = [self.recording_topics[x] for x in selections]
self.recorder = subprocess.Popen(['rosrun', 'rosbag',
'record', '-O', self.recording_path] + self.selected_topics)
self.update_file_information()
multi_dialog.Destroy()
else:
file_dialog.Destroy()
def __init__(self):
self.server_name = 'MaterialLoad'
self._result = mtconnect_msgs.msg.MaterialLoadResult()
self._as = actionlib.SimpleActionServer('MaterialLoadClient', mtconnect_msgs.msg.MaterialLoadAction, self.execute_cb, False)
self._as.start()
self._as.accept_new_goal()
self.counter = 1
# Subscribe to CNC state topic
self.door_state = None
self.chuck_state = None
self.close_door = None
# Check for CncResponseTopic
dwell = 2
while True:
published_topics = dict(rospy.get_published_topics())
if '/CncResponseTopic' in published_topics.keys():
rospy.loginfo('ROS CncResponseTopic available, starting subscriber')
break
else:
rospy.loginfo('ROS CncResponseTopic not available, will try to subscribe in %d seconds' % dwell)
time.sleep(dwell)
# Create ROS Subscriber thread
sub_thread = threading.Thread(target = self.subscriber_thread)
sub_thread.daemon = True
sub_thread.start()
def __init__(self, name):
# wait for moveit
while not "/move_group/result" in dict(
rospy.get_published_topics()).keys():
rospy.sleep(2)
self.group = MoveGroupCommander("arm")
self.group.set_start_state_to_current_state()
self.group.set_planner_id("RRTConnectkConfigDefault")
self.group.set_pose_reference_frame("/base_footprint")
self.group.set_max_velocity_scaling_factor(1)
self.group.set_num_planning_attempts(50)
self.group.set_planning_time(10)
self.group.set_goal_position_tolerance(0.01)
self.group.set_goal_orientation_tolerance(0.02)
self.tf_listener = TransformListener()
self._action_name = name
self._as = actionlib.SimpleActionServer(
self._action_name,
elevator.msg.SimpleTargetAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
def scanner_topic_check(self):
topic = ['/' + ROBOT_ID + '/scan', 'sensor_msgs/LaserScan']
last_seq_scanner = self.scanner_data.header.seq
if (topic not in rospy.get_published_topics('/' + ROBOT_ID)):
self.check_msg.data = False
self.check_pub.publish(self.check_msg)
print(
'Scanner Topic Not Registered!'
) # When Laser scanner node on robot is started without publishing the topic
return
else:
#r = rospy.Rate(10)
while (True):
self.scanner_data.header.seq == last_seq_scanner
timer = time.time()
while (self.scanner_data.header.seq == last_seq_scanner):
rospy.sleep(0.2)
if ((time.time() - timer) >= self.time_limit):
self.check_msg.data = False
self.check_pub.publish(self.check_msg)
print('Scanner Topic Stopped!'
) # When topic was already publishing
#r.sleep()
return
else:
self.check_msg.data = True
self.check_pub.publish(self.check_msg)
print('Scanner is Online')
return
def __init__(self, passive=False):
self.ranges = []
self.lstart = 0
self.lend = 0
self.lstep = 0
self.lmax = 0
self.lmin = 0
self.pose = None #x,y,heading
self.pts = None
self.count = 0
rospy.init_node('bot_int', anonymous=True)
pubbed_topics = rospy.get_published_topics()
# Prefer forwarded topics, if available.
if "/hforward/scan" in [tinfo[0] for tinfo in pubbed_topics]:
self.laser_topicname = "/hforward/scan"
else:
self.laser_topicname = "/hokuyod_client_node/scan"
if passive:
self.mtr_cont = None
# N.B., it suffices to set mtr_cont to None because any
# attempt to use mtr_cont as a Publisher object should
# fail in the form of an uncaught exception, i.e., it is
# incorrect to instantiate BotInterface in "passive mode"
# and then try to drive somewhere.
else:
self.mtr_cont = rospy.Publisher('/track_input', ldr_tracks)
rospy.sleep(.1) # apparently need to wait for >0.05 'seconds'
def expand_regex_to_topics(self, topics):
expanded_topics = []
published_topics = [t[0] for t in rospy.get_published_topics()]
for pattern in topics:
exp = re.compile(pattern)
expanded_topics += filter(lambda t: exp.match(t) is not None, published_topics)
return expanded_topics
def refresh_topics(self):
self.published_topics_and_types = rospy.get_published_topics()
if not self.check_raw_topics:
self.published_topics = [topic_name for topic_name, topic_type in self.published_topics_and_types if topic_type == 'sensor_msgs/CompressedImage']
else:
self.published_topics = [topic_name for topic_name, topic_type in self.published_topics_and_types if (topic_type == 'sensor_msgs/CompressedImage' or topic_type == 'sensor_msgs/Image')]
self.published_topics.sort()
rospy.loginfo("Found topics:\n" +
str(self.published_topics))
self.dropdown = gui.DropDown(-1, -1)
choose_ddi = gui.DropDownItem(-1, -1, "Choose topic...")
self.dropdown.append(0, choose_ddi)
for idx, topic_name in enumerate(self.published_topics):
ddi = gui.DropDownItem(-1, -1, topic_name)
self.dropdown.append(idx+1, ddi)
self.dropdown.set_on_change_listener(self, 'on_dropdown_change')
# using ID 2 to update the dropdown
self.hor_topics.append(3, self.dropdown)
# This makes the dropdown not be left
self.dropdown.style['display'] = 'block'
self.dropdown.style['margin'] = '10px auto'
self.dropdown.style['float'] = 'none'
# Force to re-render the pause button after the topics list
self.hor_topics.append(4, self.bt_pause)
self.bt_pause.style['display'] = 'block'
self.bt_pause.style['margin'] = '10px auto'
self.bt_pause.style['float'] = 'none'
self.wid.append(1, self.hor_topics)
# Re-render
if self.rosvideo_widget:
self.wid.append(2, self.rosvideo_widget)
def camera_check(self):
topics = rospy.get_published_topics()
text = ''.join(str(t) for topic in topics for t in topic)
if re.match('.*?(cam2/camera).*?', text) != None:
return True
else:
return False
def get_topics(ttyp='sensor_msgs/Image'):
topics_and_types = rospy.get_published_topics()
topics = []
for top, typ in topics_and_types:
if typ == ttyp:
topics.append(top)
return topics
def pullInHierarchy(self):
"""Attempts to pull a published hierarchy into the Abstract Map"""
hierarchy_topic = rospy.get_param('hierarchy_topic', '/hierarchy')
if next(
(t
for t in rospy.get_published_topics() if t[0] == hierarchy_topic),
None) is not None:
# Reconstruct the hierarchy
hierarchy = pickle.loads(
rospy.wait_for_message('/hierarchy',
std_msgs.String,
timeout=1.0).data)
# Add the associated symbolic spatial information strings
for h in hierarchy:
if h[1] is not None:
ssi = "%s is in %s" % (h[0], h[1])
self._abstract_map.addSymbolicSpatialInformation(
ssi, None, immediate=True)
rospy.loginfo("Added symbolic spatial information: %s" %
(ssi))
# Because we pulled in an entire hierarchy, we should finish by
# initialising the state of the entire network (this allows us to
# layout the network WITH correct mass levels, whereas the adding
# done above is not able to guarantee this...)
self._abstract_map._spatial_layout.initialiseState()
self._update_coem()
else:
rospy.logwarn("Hierarchy not available; continuing without")
def get_command(message):
cmd = message.data
if cmd == "rosnode list":
nodes = rosnode.get_node_names()
nodes.sort()
output = "\n" + "\n".join(nodes) + "\n"
rospy.loginfo(output)
if cmd == "rostopic list":
topics = rospy.get_published_topics()
topics.sort(key=lambda x: x[0])
output = "\n" + "\n".join([x.ljust(60) + y
for (x, y) in topics]) + "\n"
rospy.loginfo(output)
if cmd == "uptime":
seconds = (rospy.get_rostime() - start_time).secs
hours = int(seconds / 3600)
seconds -= hours * 3600
minutes = int(seconds / 60)
seconds -= minutes * 60
rospy.loginfo(("Uptime has reached {} hours, {} minutes, " +
"{} seconds.").format(hours, minutes, seconds))
def __init__(self):
rospy.init_node(node_name)
setup_wheels(wheels)
setup_buttons()
rate = rospy.Rate(1)
cnt = 0
while cnt < 10:
print("...")
t_arr = rospy.get_published_topics()
for i in range(len(t_arr)):
print(t_arr[i][0])
topics_arr.append(t_arr[i][0])
if ("/" + topic) in topics_arr:
cnt = 0
break
cnt = cnt + 1
rate.sleep()
if cnt == 0:
print("topic_found! \n")
self.sub = rospy.Subscriber(topic, Float32, self.distance_callback)
move_forward(wheels)
rospy.spin()
else:
return
def remove_obstacles(self, prefix_names: Union[list, None] = None):
"""remove all the obstacless belong to specific groups.
Args:
prefix_names (Union[list,None], optional): a list of group names. if it is None then all obstacles will
be deleted. Defaults to None.
"""
if len(self.obstacle_name_list) != 0:
if prefix_names is None:
group_names = '.'
re_pattern = "^(?:" + '|'.join(prefix_names) + r')\w*'
r = re.compile(re_pattern)
to_be_removed_obstacles_names = list(
filter(r.match, self.obstacle_name_list))
for n in to_be_removed_obstacles_names:
self.remove_obstacle(n)
self.obstacle_name_list = list(set(self.obstacle_name_list)-set(to_be_removed_obstacles_names))
else:
# it possible that in flatland there are still obstacles remaining when we create an instance of
# this class.
topics = rospy.get_published_topics()
for t in topics:
# the format of the topic is (topic_name,message_name)
topic_name = t[0]
object_name = topic_name.split("/")[-1]
if object_name.startswith(self._obstacle_name_prefix):
self.remove_obstacle(object_name)
def intro():
print("\nTopics:")
topics = rospy.get_published_topics()
i = 0
topic_list = []
type_list = []
for topic in topics:
topic_name = topic[0]
topic_name = topic_name[1:]
topic_type = topic[1]
topic_type = topic_type.split("/", 1)
#Sort Topics by type
if topic_type[1] == "CompressedImage" or topic_type == " Image":
print i, ") \t", topic_name
topic_list.append(topic_name)
type_list.append(topic_type[1])
i += 1
#Choosing a topic number
a = int(raw_input("\n\nPick A topic number: "))
if a < 0 or a >= i:
assert "Invalid Option"
else:
print topic_list[a], type_list[a]
image_topic = topic_list[a]
image_type = type_list[a]
print(chr(27) + "[2J")
print "\n\nA new window will open displaying the images from topic:", image_topic, "\n\n"
return image_topic, image_type
def simulator_running():
"""Helper Returns True if there is a /gazebo/link_states topic otherwise False"""
for t in rospy.get_published_topics():
if t[0] == '/gazebo/link_states':
# This is the simulator
return True
return False
def __init__(self, res_mode="sd"):
""" Constructor """
self._depth = None
self._ir = None
self._rgb = None
self._model = None
assert (res_mode in ['hd', 'sd', 'qhd'])
self._KINECT_DEPTH_TOPIC = "/kinect2/%s/image_depth_rect" % res_mode
self._KINECT_IR_TOPIC = "/kinect2/sd/image_ir_rect"
self._KINECT_COLOR_TOPIC = "/kinect2/%s/image_color_rect" % res_mode
# Get flat list of topics
topics = [t for l in rospy.get_published_topics() for t in l]
if self._KINECT_DEPTH_TOPIC not in topics:
raise ValueError("Topic %s not found" % self._KINECT_DEPTH_TOPIC)
if self._KINECT_COLOR_TOPIC not in topics:
raise ValueError("Topic %s not found" % self._KINECT_COLOR_TOPIC)
if self._KINECT_IR_TOPIC not in topics:
raise ValueError("Topic %s not found" % self._KINECT_IR_TOPIC)
self._depth_receiver = rospy.Subscriber(self._KINECT_DEPTH_TOPIC,
Image, self._depth_callback)
self._rgb_receiver = rospy.Subscriber(self._KINECT_COLOR_TOPIC, Image,
self._rgb_callback)
self._ir_receiver = rospy.Subscriber(self._KINECT_IR_TOPIC, Image,
self._ir_callback)
# Set up redis Client
self.redisClient = redis.Redis()
self.redisClient.set('env_connected', 'False')
self.invert = False
def run(self):
"""
Code to be executed when in start state
"""
try:
self.ser = serial.Serial('tty.usbmodem14101', 14101, timeout=1)
if (ser.read()):
print 'serial open and Arduino connected'
self.serialExist = true
else:
print 'serial closed and Arduino not connected'
ser.close()
except serial.serialutil.SerialException:
print 'exception'
published_topics = rospy.get_published_topics()
for (i, j) in published_topics:
if i == "Odometry":
self.odometryExist = true
if i == "scan":
self.scanExist = true
if (self.serialExist and self.odometryExist and self.scanExist):
pub = rospy.Publisher("start_mapping", Float32, queue_size=1)
rate = rospy.Rate(10)
pub.publish(1.0)
rate.sleep()
print("start_state::body")
def command():
topics = rospy.get_published_topics()
if HUSKY_TOPIC not in topics:
# launch the relevant ros topics
subprocess.Popen(["roslaunch", "husky_control", "control.launch"])
# publish to the 'cmd_vel' ROS topic
p = rospy.Publisher(HUSKY_TOPIC, Twist, queue_size=100)
rospy.sleep(TIMEOUT)
# create a twist message, and set up the linear and angular
# velocity arguments
twist = Twist()
twist.linear.x = LINEAR_VELOCITY
twist.angular.z = ANGULAR_VELOCITY
# announce move, and publish the message
rospy.loginfo("In Motion!")
# Publish Rate - Husky moves for MOVE_TIME * SLEEP_RATE seconds
for i in range(MOVE_TIME):
p.publish(twist)
rospy.sleep(SLEEP_RATE)
# create a new message
twist = Twist()
rospy.loginfo("Stopping now!")
p.publish(twist)
def __init__(self):
#Declare the variables required for the program
self.map = OccupancyGrid()
self.metadata = MapMetaData()
self.pose = PoseWithCovarianceStamped()
self.point = Point()
self.droneArray = []
self.NUMBER_OF_ILLETERATIONS = 1000
self.pheromonePath = []
rospy.loginfo("Starting aco_ros...")
##########################Drone Objects#############################
self.myTopics = [[]] #Expect a double array in rospyPubList
self.myTopics = rospy.get_published_topics()
matching = [element for element in self.myTopics if "/droneLaser" in element]
matching.sort()
droneNumber = len(matching) #Match the amount of drones to the amount of laserscans we found
#rospy.loginfo(str(droneNumber))
#################Subscribers for maps ##############################
rospy.Subscriber("/map",OccupancyGrid, self.callForMap)
#rospy.Subscriber("/map_metadata",MapMetaData,self.callForMetadata)
rospy.Subscriber("/initialpose",PoseWithCovarianceStamped, self.callFinalPose)
rospy.Subscriber("/goal",Point,self.callPoint)
####################################################Run calcuations
if (droneNumber != 0):
i = 1 #counter
while (i != droneNumber): #Instantciate an array of drone Objects
self.droneArray[i] = Drone(i,"drone_"+str(i),True,False,self.point,"/myDrone") #Set drone name and set the sensors that work at the moment
rospy.loginfo("Found drone" + str(i))
i += 1
self.main()
else:
rospy.loginfo("No drones to spawn!")
def update_time_subs(self):
for topic_name, msg_type in rospy.get_published_topics():
if 'read_until' in topic_name and 'std_msgs/Header' in msg_type:
if topic_name not in self.time_subs:
print 'ADD', topic_name
self.time_subs[topic_name] = rospy.Subscriber(
topic_name, Header, self.time_callback, topic_name)
def __init__(self):
self.measurements_per_sensor = int(rospy.get_param('~operations_per_sensor', 10))
self.sensors_ns = rospy.get_param('~sensors_namespace') # </your/namespace/to/>sensor_x
sensors_etc/ssls = set()
sensors_topics = rospy.get_published_topics(namespace=self.sensors_ns)
for topic, _ in sensors_topics:
ns_end = topic.find('sensor')
tail = topic[ns_end:]
name_end = tail.find('/')
name = tail[:name_end]
name_with_ns = topic[:topic.find(name) + len(name)]
sensors_etc/ssls.add(name_with_ns)
self.sensors_etc/ssls = list(sensors_etc/ssls)
self.sensorsProxies = list()
rospy.loginfo('Connecting to sensors proxies:')
for topic in self.sensors_etc/ssls:
rospy.loginfo(topic)
rospy.wait_for_service(topic + '/measure')
self.sensorsProxies.append(rospy.ServiceProxy(topic + '/measure', EmptySrv))
rospy.loginfo('Connecting to agent...')
rospy.wait_for_service('/agent/allow_finish')
self.finishBlockchainization = rospy.ServiceProxy('/agent/allow_finish', EmptySrv)
rospy.loginfo('Connecting to observer...')
rospy.wait_for_service('/observer/reset/')
self.resetObserver = rospy.ServiceProxy('/observer/reset', EmptySrv)
rospy.loginfo('Simulation API started.')
def listener():
rospy.init_node("listener", anonymous=True)
topic_list = rospy.get_published_topics()
if topic_list is None:
#rospy.logerr("No topics found, quitting.")
messageBox.insert(END, "No topics found, quitting.\n")
return -1
else:
#rospy.loginfo("Subscribing to topics.")
messageBox.insert(END, "Subscribing to topics.\n")
for topic_name, topic_type in topic_list:
if topic_name == "/fsrs":
rospy.Subscriber(topic_name, Vector3, fsrsListenerCallback)
FSRCanvasLabel.config(foreground="green")
if topic_name == "/accel":
rospy.Subscriber(topic_name, Vector3, accelListenerCallback)
accelLabel.config(foreground="green")
IMUCanvasLabelRoll.config(foreground="green")
IMUCanvasLabelPitch.config(foreground="green")
elif topic_name == "/magnet":
rospy.Subscriber(topic_name, Vector3, magnetListenerCallback)
magnetLabel.config(foreground="green")
elif topic_name == "/heading":
rospy.Subscriber(topic_name, Float32, headingListenerCallback)
#IMUCanvasLabelHeading.config(foreground="green")
IMUCanvasLabelYaw.config(foreground="green")
elif topic_name == "/gyro":
rospy.Subscriber(topic_name, Vector3, gyroListenerCallback)
gyroLabel.config(foreground="green")
#rospy.spin()
return 0
def dispUpdate(self):
topics = rospy.get_published_topics()
ros2Cmd = ". /opt/ros/melodic/setup.sh; . /opt/ros/dashing/setup.sh; ros2 topic list -t"
ros2result = subprocess.check_output(ros2Cmd, shell=True)
for rs2Topic in ros2result.split('\n')[1:]:
try:
topics.append([
rs2Topic.split()[0].strip("[]"),
rs2Topic.split()[1].strip("[]")
])
except:
pass
#print(rs2Topic, "is not parsed")
#print(rs2Topic)
self.tableWidget.setRowCount(len(topics))
for idx, item in enumerate(topics):
try:
self.tableWidget.setItem(idx, 0,
QTableWidgetItem(str(item[0])))
self.tableWidget.setItem(idx, 1,
QTableWidgetItem(str(item[1])))
for i in range(self.tableWidget_2.rowCount()):
#print(str(item[0]), str(self.tableWidget_2.item(i, 0).text()))
if str(item[0]) == str(
self.tableWidget_2.item(i, 0).text()):
#print("SKIP")
self.tableWidget.item(idx, 0).setBackground(
QtGui.QColor(100, 100, 150))
self.tableWidget.item(idx, 1).setBackground(
QtGui.QColor(100, 100, 150))
except:
pass
def __init__(self, topics):
''' Camera class gets images from live video and transform them
in order to predict the digit in the image.
A control thread is not necessary (the subscribers are controlled
by rospy threads).
'''
# rospy.init_node('ROSCam', anonymous=True)
# Check the existance of the topics
topic_names = zip(*rospy.get_published_topics())[0]
if topics['rgb'] not in topic_names or topics[
'depth'] not in topic_names:
raise NotAdvertisedException
# Two bridges for concurrency issues
self.rgb_bridge = cv_bridge.CvBridge()
self.depth_bridge = cv_bridge.CvBridge()
# Placeholders
self.rgb_img = np.zeros((IMAGE_HEIGHT, IMAGE_WIDTH))
self.depth_img = np.zeros((IMAGE_HEIGHT, IMAGE_WIDTH))
# Subscribers
self.rgb_lst = rospy.Subscriber(topics['rgb'],
Image,
self.__rgbCallback,
queue_size=1)
self.d_lst = rospy.Subscriber(topics['depth'],
Image,
self.__depthCallback,
queue_size=1)
self.lock_rgb = threading.Lock()
self.lock_depth = threading.Lock()
def poll(self):
"""Poll for registered sockets."""
socks = dict(self.poller.poll())
for sock in socks:
if sock == self.reg_service:
msg = self.reg_service.recv_multipart()
# Check if the topic is acturally published.
topic_list = rospy.get_published_topics()
topics = [x[0] for x in topic_list]
if msg[0] not in topics:
self.reg_service.send(b'-1')
else:
self.reg_service.send(b'0')
data_class = load_class(msg[2], msg[1])
sub = rospy.Subscriber(msg[0],
data_class,
self.handle_subscription,
msg[0])
self.subs[msg[0]] = sub
if sock == self.sub_socket:
msg = self.sub_socket.recv_multipart()
if msg[0] not in self.pubs:
data_class = load_class(msg[3], msg[2])
pub = rospy.Publisher(msg[0], data_class, queue_size=10)
self.pubs[msg[0]] = pub
self.pubs[msg[0]].publish(msg[1])
def _update_combo_topics(self):
"""
Update the topics displayed in the combo box that the user can use to select
which topic they want to listen on for trees, filtered so that only
topics with the correct message type are shown.
"""
# Only update topics if we're running with live updating
if not self.live_update:
self._widget.topic_combo_box.setEnabled(False)
return
self._widget.topic_combo_box.clear()
topic_list = rospy.get_published_topics()
valid_topics = []
for topic_path, topic_type in topic_list:
if topic_type == RosBehaviourTree._expected_type:
valid_topics.append(topic_path)
if not valid_topics:
self._widget.topic_combo_box.addItem(RosBehaviourTree._empty_topic)
return
# always add an item which does nothing so that it is possible to listen to nothing.
self._widget.topic_combo_box.addItem(
RosBehaviourTree._unselected_topic)
for topic in valid_topics:
self._widget.topic_combo_box.addItem(topic)
# if the topic corresponds to the one that was active the last time
# the viewer was run, automatically set that one as the one we look
# at
if topic == self._saved_settings_topic:
self._widget.topic_combo_box.setCurrentIndex(
self._widget.topic_combo_box.count() - 1)
self._choose_topic(self._widget.topic_combo_box.currentIndex())
def __init__(self):
self.server_name = 'MaterialLoad'
self._result = mtconnect_msgs.msg.MaterialLoadResult()
self._as = actionlib.SimpleActionServer(
'MaterialLoadClient', mtconnect_msgs.msg.MaterialLoadAction,
self.execute_cb, False)
self._as.start()
self._as.accept_new_goal()
self.counter = 1
# Subscribe to CNC state topic
self.door_state = None
self.chuck_state = None
self.close_door = None
# Check for CncResponseTopic
dwell = 2
while True:
published_topics = dict(rospy.get_published_topics())
if '/CncResponseTopic' in published_topics.keys():
rospy.loginfo(
'ROS CncResponseTopic available, starting subscriber')
break
else:
rospy.loginfo(
'ROS CncResponseTopic not available, will try to subscribe in %d seconds'
% dwell)
time.sleep(dwell)
# Create ROS Subscriber thread
sub_thread = threading.Thread(target=self.subscriber_thread)
sub_thread.daemon = True
sub_thread.start()
def handleMessage(msg):
global count
global df2
launch = str(msg)
if check == False:
if count == 1:
s = threading.Thread(target=roslaunch, args=(launch, ))
s.start()
time.sleep(2)
count = 2
strlist = []
while len(strlist) == 0:
lists = rospy.get_published_topics()
for i in lists:
if i[1] == 'std_msgs/String':
strlist.append(i[0])
colors = color_gen(len(strlist))
random.shuffle(colors)
for idx, j in enumerate(strlist):
df2 = df2.append({
'Topic': j,
'Color': colors[idx]
},
ignore_index=True)
df2 = df2.set_index('Topic')
y = threading.Thread(target=listener, args=(strlist, ))
y.start()
else:
print("Roscore is not running. Start roscore and try again")
jsonfile = df1.to_json(orient='records')
send(jsonfile, broadcast=True)
def create_objs_from_rostopics(self):
rospy.init_node(self._client_name)
rospy.on_shutdown(self.clean_up)
self._rate = rospy.Rate(1000)
self._ros_topics = rospy.get_published_topics()
for i in range(len(self._ros_topics)):
for j in range(len(self._ros_topics[i])):
prefix_ind = self._ros_topics[i][j].find(self._search_prefix_str)
if prefix_ind >= 0:
search_ind = self._ros_topics[i][j].find(self._search_suffix_str)
if search_ind >= 0:
# Searching the active topics between the end of prefix:/ambf/env/ and start of /State
obj_name = self._ros_topics[i][j][
prefix_ind + len(self._search_prefix_str):search_ind]
if obj_name == 'World' or obj_name == 'world':
self._world_name = obj_name
obj = World(obj_name)
obj._sub = rospy.Subscriber(self._ros_topics[i][j], WorldState, obj.ros_cb)
pub_topic_str = self._search_prefix_str + obj_name + self._string_cmd
obj._pub = rospy.Publisher(name=pub_topic_str, data_class=WorldCmd, queue_size=10)
self._world_handle = obj
else:
obj = Object(obj_name)
obj.set_name(obj_name)
obj._sub = rospy.Subscriber(self._ros_topics[i][j], ObjectState, obj.ros_cb)
pub_topic_str = self._search_prefix_str + obj_name + self._string_cmd
obj._pub = rospy.Publisher(name=pub_topic_str, data_class=ObjectCmd, tcp_nodelay=True, queue_size=10)
self._objects_dict[obj_name] = obj
def update(self):
self.reset()
self.rootItem = TreeItem(['name','type'])
self.list = []
for topic_name, topic_type in rospy.get_published_topics():
message_class = roslib.message.get_message_class(topic_type)
message_obj = message_class()
self._inception(topic_name, message_obj, topic_type)
def getTopicType(topic):
""" Returns the type of the specified ROS topic """
# If the topic is published, return its type
for x in rospy.get_published_topics():
if x[0]==topic:
return x[1]
# Topic isn't published so return an empty string
return ""
def get_ros_topics(self):
'''return list of published topics'''
try:
topics = rospy.get_published_topics()
return ' ,'.join((topic[0] for topic in topics))
except:
rospy.logerr("no core active")
return False
def send_topics(self):
filtered_topics = {}
for value, key in rospy.get_published_topics():
if key in input_topic_types:
filtered_topics.setdefault(key, []).append(value)
self.write_message(json.dumps(filtered_topics))
tornado.ioloop.IOLoop.instance().add_timeout(client_refresh_rate, self.send_topics)
def update_subscribers(self):
# check new publishers
topics = [t for t in rospy.get_published_topics() if self.action_regex.match(t[1])]
if self.action_name_white_list:
topics = [t for t in topics if t[0] in self.action_name_white_list]
if self.action_type_white_list:
topics = [t for t in topics if t[1] in self.action_type_white_list]
if self.action_name_black_list:
topics = [t for t in topics if not t[0] in self.action_name_black_list]
if self.action_type_black_list:
topics = [t for t in topics if not t[1] in self.action_type_black_list]
for topic_name, topic_type in topics:
(pkg_name, msg_type) = topic_type.split('/')
py_topic_class = '%s.msg.%s' % (pkg_name, msg_type)
if py_topic_class in self.useless_types:
continue
if topic_name in self.subscribers.keys():
continue
# Import and Check
try:
pypkg = __import__('%s.msg' % pkg_name)
rospy.logdebug('imported %s.msg', pkg_name)
except Exception as e:
rospy.logerr('failed to import %s.msg: %s', pkg_name, e)
rospy.logerr('please catkin_make %s', pkg_name)
self.useless_types += [py_topic_class]
continue
try:
type_class = getattr(getattr(pypkg, 'msg'), msg_type)
type_instance = type_class()
except Exception as e:
rospy.logerr('failed to instantiate %s.msg.%s: %s', pkg_name, msg_type, e)
self.useless_types += [py_topic_class]
continue
try:
cb_func = self._get_callback(topic_name, msg_type, type_instance)
if cb_func is None:
self.useless_types += [py_topic_class]
continue
self.subscribers[topic_name] = rospy.Subscriber(topic_name, type_class,
cb_func, queue_size=self.queue_size)
rospy.logdebug("start subscribe (topic=%s type=%s)", topic_name, msg_type)
except Exception as e:
self.useless_types += [py_topic_class]
rospy.logerr('error registering subscriber: %s', e)
continue
# cleanup subscribers
cleanup_topics = set(self.subscribers.keys()) - set([t[0] for t in topics])
for t in cleanup_topics:
self.subscribers[t].unregister()
del self.subscribers[t]
rospy.logdebug("unsubscribe %s" % t)
def trigger_configuration(self):
topics = [t for t in rospy.get_published_topics() if t[1] == 'rosgraph_msgs/Log']
topics.sort(key=lambda tup: tup[0])
dialog = ConsoleSettingsDialog(topics, self._rospack)
(topic, message_limit) = dialog.query(self._topic, self._model.get_message_limit())
if topic != self._topic:
self._subscribe(topic)
if message_limit != self._model.get_message_limit():
self._model.set_message_limit(message_limit)
def is_topic_pub(topic):
flag = False
for item in rospy.get_published_topics():
for thing in item:
if topic in thing:
flag = True
else:
pass
return flag
def timer_cb(self, event=None):
all_topics = rospy.get_published_topics()
msg_types = [
'InteractiveMarker',
'InteractiveMarkerControl',
'InteractiveMarkerInit',
'InteractiveMarkerUpdate',
'Marker',
'MarkerArray',
]
topic_names = []
topic_types = []
for topic in all_topics:
if topic[0].split('/')[-2] == self.suffix:
continue
for msg_type in msg_types:
if topic[1] == 'visualization_msgs/{}'.format(msg_type):
topic_names.append(topic[0])
topic_types.append(
getattr(visualization_msgs.msg, msg_type))
for topic_name, topic_type in zip(topic_names, topic_types):
if topic_name not in self.publishers:
new_topic_name = '/'.join(
topic_name.split('/')[:-1] +
[self.suffix, topic_name.split('/')[-1]])
self.publishers[topic_name] = rospy.Publisher(
new_topic_name, topic_type, queue_size=1)
rospy.logdebug(
'Advertised: {0} -> {1}'.format(
topic_name, new_topic_name))
for topic_name, pub in self.publishers.items():
# clean old topics
if topic_name not in topic_names:
try:
self.subscribers.pop(topic_name).unregister()
rospy.logdebug('Removed subscriber: {}'.format(topic_name))
except Exception:
pass
try:
self.publishers.pop(topic_name).unregister()
rospy.logdebug('Removed publisher: {}'.format(topic_name))
except Exception:
pass
# subscribe topics subscribed
elif pub.get_num_connections() > 0:
if topic_name not in self.subscribers:
self.subscribers[topic_name] = rospy.Subscriber(
topic_name, rospy.AnyMsg, self.msg_cb, topic_name)
rospy.logdebug('Subscribed {}'.format(topic_name))
# unsubscribe topics unsubscribed
else:
if topic_name in self.subscribers:
self.subscribers.pop(topic_name).unregister()
rospy.logdebug('Unsubscribed {}'.format(topic_name))
def find_topics_w_ending(name):
"""
Finds a list of all topics ending in the given name
"""
topic_list = [n for n,t in rospy.get_published_topics()]
topic_w_name_list = []
for topic in topic_list:
if name == topic.split('/')[-1]:
topic_w_name_list.append(topic)
return topic_w_name_list
def get_msg_class_dict(self, topic_names):
all_topics = dict(rospy.get_published_topics())
msg_class_dict = {}
for topic_name in topic_names:
class_type_string = all_topics[topic_name]
topic_package_name, topic_class_name = class_type_string.split('/')
topic_package = eval('__import__("%s.msg"%(topic_package_name), fromlist=[topic_class_name], level=-1)')
msg_class = eval('topic_package.%s'%(topic_class_name))
msg_class_dict[topic_name] = msg_class
return msg_class_dict
def find_topics_w_name(name):
"""
Finds a list of all topics containing the given name
"""
topic_list = [n for n,t in rospy.get_published_topics()]
topic_w_name_list = []
for topic in topic_list:
if name in topic.split('/'):
topic_w_name_list.append(topic)
return topic_w_name_list
def searchForLandmarkUpdates(self, event):
# rospy.loginfo("%s: Search for new landmark updates ...", self.name)
topics = rospy.get_published_topics('/pose_ekf_slam/landmark_update')
for t in topics:
if t[1] == 'geometry_msgs/PoseWithCovarianceStamped':
if t[0] not in self.landmark_update_topics:
rospy.loginfo('Subscribe to landmark update: %s', t[0])
self.landmark_update_topics.append(t[0])
rospy.Subscriber(t[0], PoseWithCovarianceStamped,
self.landmarkUpdate, t[0])
def get_number_of_frame_skipper_topics():
"""
Returns the number of currently published frame skipper topics.
"""
topic_list = rospy.get_published_topics()
cnt = 0
for topic in topic_list:
topic_end = topic[0].split('/')[-1]
if 'skip' in topic_end:
cnt+= 1
return cnt
def update_topics(self):
self.model().clear()
topic_list = rospy.get_published_topics()
for topic_path, topic_type in topic_list:
topic_name = topic_path.strip('/')
message_class = roslib.message.get_message_class(topic_type)
if message_class is None:
qWarning('TopicCompleter.update_topics(): could not get message class for topic type "%s" on topic "%s"' % (topic_type, topic_path))
continue
message_instance = message_class()
self.model().add_message(message_instance, topic_name, topic_type, topic_path)
def refresh_topics(self):
'''
Refresh topic list in the combobox
'''
topic_list = rospy.get_published_topics()
if topic_list is None:
return
self.topic_combox.clear()
for (name, type) in topic_list:
if type == 'trajectory_msgs/JointTrajectory':
self.topic_combox.addItem(name)
def get_number_of_corrector_topics():
"""
Returns the number of currently published frame drop corrector "seq_and_image_corr"
topics
"""
topic_list = rospy.get_published_topics()
cnt = 0
for topic in topic_list:
topic_end = topic[0].split('/')[-1]
if 'seq_and_image_corr' in topic_end:
cnt+= 1
return cnt
def find_camera_image_topics(transport='image_raw'):
"""
Finds a list of all active camera topics
"""
topic_list = rospy.get_published_topics()
image_topic_list = []
for topic in topic_list:
topic_name, topic_type= topic
topic_name_split = topic_name.split('/')
if (topic_name_split[-1] == transport) and ('camera' in topic_name_split):
image_topic_list.append(topic_name)
return image_topic_list
def find_camera_info_topics():
"""
Returns a list of all camera info topics
"""
topic_list = rospy.get_published_topics()
info_topic_list = []
for topic in topic_list:
topic_name, topic_type = topic
topic_name_split = topic_name.split('/')
if 'camera_info' in topic_name_split:
info_topic_list.append(topic_name)
return info_topic_list
