def OnClicktopicpub(self, event):
if self.client.is_connected:
strpubname = str(self.m_texttopicpubname.GetValue())
strpubmessagetype = str(self.m_texttopicpubmessagetype.GetValue())
strpubmessage = str(self.m_texttopicpubmessage.GetValue())
dictpub = ''
try:
dictpub = eval(strpubmessage)
except Exception as e:
if self.timer_publish.IsRunning():
self.timer_publish.Stop()
self.m_checkBox_auto.SetValue(False)
dlg = wx.MessageDialog(None, '消息格式错误，请检查是否为json格式', '消息格式错误',
wx.OK)
if dlg.ShowModal() == wx.ID_OK:
pass
else:
pass
dlg.Destroy()
if strpubname is not '':
if strpubmessagetype is not '':
if self.topic_pub is None:
self.topic_pub = roslibpy.Topic(
self.client, strpubname, strpubmessagetype)
self.topic_pub.publish(roslibpy.Message(dictpub))
else:
if self.topic_pub.name is not strpubname:
if self.topic_pub.is_advertised:
self.topic_pub.unadvertise()
self.topic_pub = roslibpy.Topic(
self.client, strpubname, strpubmessagetype)
self.topic_pub.publish(roslibpy.Message(dictpub))
async def setup_rosbridge(self) : # connect to rosbridge self.rosbridge_client = roslibpy.Ros(host=self.rosbridge_address, port=self.rosbridge_port) await self.attempt(self.rosbridge_client.run, check=(lambda x : self.rosbridge_client.is_connected), sync=True, timeout=1) # create client for frame service self.frame_service = roslibpy.Service(self.rosbridge_client, self.frame_req_service_name, self.frame_req_msg_type) # subscribe to frame topic self.frame_subscriber = roslibpy.Topic(self.rosbridge_client, self.frame_resp_topic_name, self.frame_resp_msg_type) self.frame_subscriber.subscribe(lambda msg: self.subscriber_callback(msg)) # setup publisher for storage goal topic self.storage_publisher = roslibpy.Topic(self.rosbridge_client, self.storage_req_topic_name, self.storage_req_msg_type) self.storage_publisher.advertise() # subscribe to storage result topic self.storage_subscriber = roslibpy.Topic(self.rosbridge_client, self.storage_resp_topic_name, self.storage_resp_msg_type) self.storage_subscriber.subscribe(lambda msg: self.subscriber_callback(msg)) # setup publisher for registration goal topic self.registration_publisher = roslibpy.Topic(self.rosbridge_client, self.registration_req_topic_name, self.registration_req_msg_type) self.registration_publisher.advertise() # subscribe to registration result topic self.registration_subscriber = roslibpy.Topic(self.rosbridge_client, self.registration_resp_topic_name, self.registration_resp_msg_type) self.registration_subscriber.subscribe(lambda msg: self.subscriber_callback(msg))
def __init__(self):
# base robot ip and port
self.__base_port = rospy.get_param('~base_port', 9090)
self.__base_ip = rospy.get_param('~base_address', 'scout-mini-02.local')
# robot name, each robot should have different name
self.__robot_name = rospy.get_param('~robot_name', 'scout-mini-02')
self.__robot_frame_id = rospy.get_param('~robot_frame_id', 'base_footprint')
self.__load_swarm_trans()
self.__current_command = 'dismiss'
self.__tf_listener = tf.TransformListener()
self.__ros_client = roslibpy.Ros(self.__base_ip, self.__base_port)
self.__joy_command = roslibpy.Topic(self.__ros_client, 'yocs_cmd_vel_mux/input/joy_cmd', 'geometry_msgs/Twist')
self.__swarm_command = roslibpy.Topic(self.__ros_client, 'swarm_command', 'swarm_msgs/SwarmCommand')
self.__swarm_heartbeat = roslibpy.Topic(self.__ros_client, 'swarm_hearbeat', 'swarm_msgs/SwarmHeartbeat')
# self.__move_base_pub = rospy.Publisher('swarm_goal', PoseStamped, queue_size=30)
self.__move_base_pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=30)
self.__cmd_pub = rospy.Publisher('yocs_cmd_vel_mux/input/navigation_cmd', Twist, queue_size=30)
# self.__cmd_pub = rospy.Publisher('test_cmd', Twist, queue_size=30)
self.__target_pose = PoseStamped()
self.__swarm_base_pose = Pose()
self.__ros_client.on_ready(self.__start_sending, run_in_thread=True)
self.__ros_client.on_ready(self.__start_receiving, run_in_thread=True)
self.__ros_client.run()
rospy.loginfo('Swarm client started')
def get_params():
print("ROS connected:", ros.is_connected)
robot_id_param = roslibpy.Param(ros, "/QTCStatePublisherNode/robot_id")
global robot_id
global qtcTopic
global sitTopic
global rejection_KL_thresh
robot_id = robot_id_param.get()
print(str(robot_id))
rejection_thresh_param = roslibpy.Param(
ros, "/robot" + str(robot_id) + "/qsr/situation_rejection_threshold")
rejection_KL_thresh = rejection_thresh_param.get()
qtcTopic = roslibpy.Topic(ros, "/robot" + str(robot_id) + "/qsr/qtc_state",
"std_msgs/String")
sitTopic = roslibpy.Topic(
ros, "/robot" + str(robot_id) + "/qsr/situation_predictions",
"std_msgs/String")
print(qtcTopic.name)
print(sitTopic.name)
# Current and recent human and then robot positions and angles:
# [xh0, yh0, hh0, xh1, yh1, hh1, xr0, yr0, hr0, xr1, yr1, hr1]
qtcTopic.subscribe(lambda message: qtc_update_callback(message))
def start_ros_topics(self):
print("Starting ROS Topics2")
# subscribers
self.subscribers = []
# this is a dict containing the district assignments, histograms, and metrics
# this is published every time that a fiducial moves
sub_designs_topic = roslibpy.Topic(self.ros, '/evaluated_designs',
'std_msgs/String')
sub_designs_topic.subscribe(self.handle_evaluated_designs)
self.subscribers.append(sub_designs_topic)
# # this contains block locations and is fired when fiducials move
# sub_blocks_topic = roslibpy.Topic(
# self.ros, '/blocks', 'std_msgs/String')
# sub_blocks_topic.subscribe(
# self.handle_blocks)
# self.subscribers.append(sub_blocks_topic)
# this contains messages about the focus. only triggered when the focus block moves
sub_tuio_topic = roslibpy.Topic(self.ros, '/tuio_control',
'std_msgs/String')
sub_tuio_topic.subscribe(self.handle_tuio_control)
self.subscribers.append(sub_tuio_topic)
#logging.info('Started ros-bridge publishers')
# publisher to move the focus
self.pub_focus_action_topic = roslibpy.Topic(self.ros, '/focus_action',
'std_msgs/Int8')
self.pub_focus_action_topic.advertise()
print("running")
def __init__(self, ros, namespace='/rob1'):
"""Initialize a new robot client instance.
Parameters
----------
ros : :class:`RosClient`
Instance of a ROS connection.
namespace : :obj:`str`
Namespace to allow multiple robots to be controlled through the same ROS instance.
Optional. If not specified, it will use namespace ``/rob1``.
"""
self.ros = ros
self.counter = SequenceCounter()
if not namespace.endswith('/'):
namespace += '/'
self._version_checked = False
self._server_protocol_check = dict(event=threading.Event(),
param=roslibpy.Param(
ros,
namespace + 'protocol_version'),
version=None)
self.ros.on_ready(self.version_check)
self.topic = roslibpy.Topic(ros,
namespace + 'robot_command',
'compas_rrc_driver/RobotMessage',
queue_size=None)
self.feedback = roslibpy.Topic(ros,
namespace + 'robot_response',
'compas_rrc_driver/RobotMessage',
queue_size=0)
self.feedback.subscribe(self.feedback_callback)
self.topic.advertise()
self.futures = {}
self.ros.on('closing', self._disconnect_topics)
def __init__(self,robot_id):
# init HMM model
working_dir = os.path.dirname(os.path.realpath(__file__))
# Load per-situation HMMs
self.pblHMM = hmms.DtHMM.from_file(working_dir+"/../Models/pblHMM_k_3_hf.npz")
self.pbrHMM = hmms.DtHMM.from_file(working_dir+"/../Models/pbrHMM_k_3_hf.npz")
self.rotlHMM = hmms.DtHMM.from_file(working_dir+"/../Models/rotlHMM_k_3_hf.npz")
self.rotrHMM = hmms.DtHMM.from_file(working_dir+"/../Models/rotrHMM_k_3_hf.npz")
self.pclHMM = hmms.DtHMM.from_file(working_dir+"/../Models/pclHMM_k_3_hf.npz")
self.pcrHMM = hmms.DtHMM.from_file(working_dir+"/../Models/pcrHMM_k_3_hf.npz")
# Configuration parameters
self.rejection_KL_thresh = 0.025
N_nodes = 81
self.classes = ["PBL", "PBR", "ROTL", "ROTR", "PCL", "PCR"]
# Functions for mapping QTC_C states to integers
# Create list of QTC_C states so that indices can be used as integer state IDs compatible with HMM library
QTC_symbols = []
for i in range(0, 4):
QTC_symbols.append("-")
QTC_symbols.append("0")
QTC_symbols.append("+")
# print("QTC symbols:", QTC_symbols[:3])
self.QTC_C_states = list(combinations(QTC_symbols, 4))
self.QTC_C_states = [state[0] + state[1] + state[2] + state[3] for state in self.QTC_C_states]
self.QTC_C_states = list(np.unique(self.QTC_C_states))
# init param
self.prev_time = time.time()
self.qtc_seq = []
# init ros
ros_client = roslibpy.Ros(host='localhost', port=9090)
ros_client.run()
print("ROS connected:", ros_client.is_connected)
# read parameters
print("Robot Id :", robot_id)
rejection_thresh_param = roslibpy.Param(ros_client, "/robot"+str(robot_id)+"/qsr/situation_rejection_threshold")
self.rejection_KL_thresh = rejection_thresh_param.get()
print("rejection_KL_thresh:",self.rejection_KL_thresh )
# define topics
self.qtcTopic = roslibpy.Topic(ros_client, "/robot"+str(robot_id)+"/qsr/qtc_state", "std_msgs/String")
self.sitTopic = roslibpy.Topic(ros_client, "/robot"+str(robot_id)+"/qsr/situation_predictions", "std_msgs/String")
# define subscribers
self.qtcTopic.subscribe(lambda message: self.qtc_update_callback(message))
while ros_client.is_connected:
time.sleep(1)
ros_client.terminate()
def main_(bag_file):
fo = open("output_image.txt", "w")
fo.write("")
fo.close()
fo = open("output.txt", "w")
fo.write("")
fo.close()
bag_file = '/data/' + str(bag_file)
played = False
def bagPlayed(Bagdata):
global played
if (Bagdata):
played = False
fo = open("output.txt", "a")
time.sleep(1)
fo.write("bag finished playing")
print("bag finished playing")
fo.close()
client = roslibpy.Ros(host=os.environ['HOST'], port=8080)
client.run()
client.on_ready(lambda: print('Is ROS connected?', client.is_connected))
imu_listener = roslibpy.Topic(client, '/mavros/imu/data',
'sensor_msgs/Imu')
imu_listener.subscribe(lambda IMUdata: ImuCallback([
float(IMUdata['linear_acceleration']['x']),
float(IMUdata['linear_acceleration']['y']),
float(IMUdata['linear_acceleration']['z'])
], IMUdata['header']))
#image_listener = roslibpy.Topic(client, '/pylon_camera_node/image_raw', 'sensor_msgs/Image')
image_listener = roslibpy.Topic(client, '/resizedImage',
'sensor_msgs/TimeReference')
image_listener.subscribe(lambda Imagedata: ImageCallback(Imagedata))
bag_listener = roslibpy.Topic(client, '/bagEnd', 'std_msgs/Bool')
bag_listener.subscribe(lambda Bagdata: bagPlayed(Bagdata))
bag_player = roslibpy.Topic(client, '/bagStart', 'std_msgs/String')
#To play Bag file - change above bag path and call funtion below (now only runs once)
##funtion start
if (client.is_connected and (not played)):
bag_player.publish(roslibpy.Message({'data': bag_file}))
played = True
##funtion end
try:
while True:
pass
except KeyboardInterrupt:
client.terminate()
def __init__(self, ros_client: roslibpy.Ros, callback: callable, topics: Sequence[str], queue_size: int=100, allow_headerless: float=0.1):
msg_type = 'sensor_msgs/CompressedImage'
self.client_callback = callback
self.tps = [roslibpy.Topic(ros_client, name, msg_type) for name in topics]
if len(self.tps) > 1:
self.subscriber = TimeSynchronizer(self.tps, self.msg_to_cv2, queue_size, allow_headerless)
else:
self.subscriber = roslibpy.Topic(ros_client, self.tps[0], msg_type)
self.subscriber.subscribe(self.msg_to_cv2)
def __init__(self, *args, **kwargs):
self.ros = roslibpy.Ros(host=HOST, port=PORT)
self.ros_odom = roslibpy.Topic(self.ros, '/odom', 'nav_msgs/Odometry')
self.ros_vel = roslibpy.Topic(self.ros, '/cmd_vel_mux/input/teleop', 'geometry_msgs/Twist')
self.step = 0
self.prev_robot_action = np.array([0,0])
super().__init__(*args, **kwargs)
if REAL:
self.start_ros()
def __init__(self):
self.roslibpy_client = roslibpy.Ros(host='192.168.2.76', port=9099)
self.roslibpy_client.run()
self.pos_sender = roslibpy.Topic(self.roslibpy_client,
'/iiwa_joint_position',
'iiwa_msgs/JointPosition')
self.vel_sender = roslibpy.Topic(self.roslibpy_client,
'/iiwa_joint_velocity',
'iiwa_msgs/JointVelocity')
self.pose_receiver = roslibpy.Topic(self.roslibpy_client, 'FX_msg',
'geometry_msgs/PoseStamped')
self.pose_receiver.subscribe(self.callback_basepose)
rospy.init_node('communication_node')
self.vel_sub = rospy.Subscriber('iiwa/state/JointVelocity',
JointVelocity, self.call_back_vel)
self.pos_sub = rospy.Subscriber('iiwa/state/JointPosition',
JointPosition, self.call_back_pos)
self.base_pose_pub = rospy.Publisher('FX_msg_roslibpy',
PoseStamped,
queue_size=10)
self.count = 0
self.base_pose = PoseStamped()
self.joint_vel = dict()
self.joint_vel['header'] = dict()
self.joint_vel['header']['seq'] = 0
self.joint_vel['header']['frame_id'] = ''
self.joint_vel['header']['stamp'] = dict()
self.joint_vel['header']['stamp']['secs'] = 0
self.joint_vel['header']['stamp']['nsecs'] = 0
self.joint_vel['velocity'] = dict()
self.joint_vel['velocity']['a1'] = 0
self.joint_vel['velocity']['a2'] = 0
self.joint_vel['velocity']['a3'] = 0
self.joint_vel['velocity']['a4'] = 0
self.joint_vel['velocity']['a5'] = 0
self.joint_vel['velocity']['a6'] = 0
self.joint_vel['velocity']['a7'] = 0
self.joint_pos = dict()
self.joint_pos['header'] = dict()
self.joint_pos['header']['seq'] = 0
self.joint_pos['header']['frame_id'] = ''
self.joint_pos['header']['stamp'] = dict()
self.joint_pos['header']['stamp']['secs'] = 0
self.joint_pos['header']['stamp']['nsecs'] = 0
self.joint_pos['position'] = dict()
self.joint_pos['position']['a1'] = 0
self.joint_pos['position']['a2'] = 0
self.joint_pos['position']['a3'] = 0
self.joint_pos['position']['a4'] = 0
self.joint_pos['position']['a5'] = 0
self.joint_pos['position']['a6'] = 0
self.joint_pos['position']['a7'] = 0
def __init__(self, client):
"""
Constructor of the class
"""
super(Bebop, self).__init__(client, '/bebop', self.MOVE_TOPIC,
self.POSSIBLE_MOVES)
self.takeoff_topic = roslibpy.Topic(client, '/bebop/takeoff',
'std_msgs/Empty')
self.land_topic = roslibpy.Topic(client, '/bebop/land',
'std_msgs/Empty')
def __init__(self):
self.client = roslibpy.Ros(host=ROBOT_IP, port=ROBOT_PORT)
self.messenger_publisher = roslibpy.Topic(self.client,
messenger.MESSENGER_TOPIC,
messenger.MESSENGER_TYPE)
self.controller_publisher = roslibpy.Topic(self.client,
controller.CONTROLLER_TOPIC,
controller.CONTROLLER_TYPE)
self.voice_publisher = roslibpy.Topic(self.client,
voice_assistant.VOICE_TOPIC,
voice_assistant.VOICE_TYPE)
def __init__(self, ros_client: roslibpy.Ros, goal_callback: callable, odom_topic: str='/odom', map_topic: str='/map'):
self.ros_client = ros_client
self.result_callback = goal_callback
self.mb_scheduler = ActionScheduler(self.ros_client, '/move_base', 'move_base_msgs/MoveBaseAction', self.result_callback)
self.is_get_map = False
self.map_listener = roslibpy.Topic(self.ros_client, map_topic, 'nav_msgs/OccupancyGrid')
self.map_listener.subscribe(self._update_map)
self.is_get_odom = False
self.odom_listener = roslibpy.Topic(self.ros_client, odom_topic, 'nav_msgs/Odometry')
self.odom_listener.subscribe(self._update_odometry)
def __init__(self, robot):
self.ros = robot.ros
self.talker = roslibpy.Topic(self.ros, '/cmd_vel', 'geometry_msgs/msg/Twist')
self.encoder_listener = roslibpy.Topic(self.ros, '/encoder', 'motor_msgs/msg/Encoder')
self.talker.advertise()
self.encoder_listener.subscribe(self._on_encoder_msg)
self.encoders = {
'l_rpm': [0] * 200,
'r_rpm': [0] * 200,
'l_target': [0] * 200,
'r_target': [0] * 200,
}
while not self.talker.is_advertised:
time.sleep(0.1)
def setup_listeners(self):
self.imageListener = roslibpy.Topic(
self.client,
f'/{self.rover_id}/camera_node/image/compressed',
'sensor_msgs/CompressedImage',
throttle_rate=2000,
queue_length=1,
queue_size=1)
self.imageListener.subscribe(self.image_received_callback)
self.lidarListener = roslibpy.Topic(self.client,
'/scan',
'sensor_msgs/LaserScan',
throttle_rate=500,
queue_length=1,
queue_size=1)
self.lidarListener.subscribe(self.lidar_received_callback)
def __init__(self, arm_names, upstream_topic, host, port, trigger_topic, drop_rate):
self.upstream_topic = upstream_topic
self.trigger_topic = trigger_topic
self.drop_rate = drop_rate
self.cnt = 0
# Creating rosbridge connection
self.ros = roslibpy.Ros(host=host, port=port, is_secure=False)
self.ros.on_ready(self.connect_cb)
self.arms = []
# Constructing RobotArmDVRK instance for each arm
for name in arm_names:
if 'SUJ_PSM' in name:
self.arms.append(RobotArmSUJPSM(name, self.ros))
elif 'SUJ_ECM' in name:
self.arms.append(RobotArmSUJECM(name, self.ros))
elif 'ECM' in name:
self.arms.append(RobotArmECM(name, self.ros))
elif 'PSM' in name:
self.arms.append(RobotArmPSM(name, self.ros))
# Advertising rosbridge topic
self.topic = roslibpy.Topic(self.ros, self.upstream_topic, 'sensor_msgs/JointState')
self.topic.advertise()
# Start loop
self.ros.run_forever()
def setup_publishers(self):
self.joyPublisher = roslibpy.Topic(self.client,
f'/{self.rover_id}/joy',
'sensor_msgs/Joy',
queue_size=1,
queue_length=1)
self.joyPublisher.advertise()
def register_map_listener(self, callback):
self.map_listener = roslibpy.Topic(self.cli, '/map', 'nav_msgs/OccupancyGrid')
def wrapped_callback(msg):
callback(msg)
self.map_listener.subscribe(wrapped_callback)
def publish(self, topic, message_type, message):
try:
publisher = roslibpy.Topic(self.__ros, topic, message_type)
publisher.publish(roslibpy.Message(message))
except:
self.parent.print_console(
"Erreur avec la publication de votre message.")
def __init__(self):
self.in_topic = '/main_cam/image_raw/compressed'
self.out_topic = '/ofm/draw_image/compressed'
self.in_data_type = 'sensor_msgs/CompressedImage'
self.out_data_type = 'sensor_msgs/CompressedImage'
self.header = None
self.image = None
self.seq = 0
self.client = roslibpy.Ros(host='localhost', port=9090)
self.listener = roslibpy.Topic(self.client,
self.in_topic,
self.in_data_type,
throttle_rate=100,
queue_size=1)
self.talker = roslibpy.Topic(self.client, self.out_topic,
self.out_data_type)
def __init__(self):
self.client = roslibpy.Ros(host='localhost', port=9090)
self.client.run()
self.service = roslibpy.Service(self.client, 'get_images', 'exp_demo/ObjectArray')
self.publisher = roslibpy.Topic(self.client, '/indicate', 'geometry_msgs/PointStamped')
self.publisher.advertise()
def _check_for_topics(self, event_data):
"""Check to see if there are any new topics we don't know about"""
sub_topics = rospy.get_published_topics()
for topic, msg_type in sub_topics:
if topic not in self._publishers:
print('registering publisher for {}'.format(topic))
self._publishers[topic] = roslibpy.Topic(
self._roslib_client, topic,
self._roslib_client.get_topic_type(topic))
self._publishers[topic].advertise()
if topic not in self._subscribers:
print('registering subscriber for {}'.format(topic))
self._subscribers[topic] = roslibpy.Topic(
self._roslib_client, topic,
self._roslib_client.get_topic_type(topic))
self._subscribers[topic].subscribe(self._null_callback)
def __init__(self, robot):
self.ros = robot.ros
self.listener = roslibpy.Topic(self.ros, '/camera',
'sensor_msgs/msg/CompressedImage')
self.listener.subscribe(self._on_msg)
self.image = None
while self.image is None:
time.sleep(0.1)
def ros_run(self, conn):
self.conn = conn
uav_number = conn.recv()
ros = roslibpy.Ros(host='localhost', port=9090)
ros.on_ready(lambda: print('Is ROS Connected?', ros.is_connected))
roslibpy.Topic(ros, '/cv{0}/output_raw'.format(uav_number),
'sensor_msgs/Image').subscribe(self.callback)
ros.run_forever()
def __init__(self, robot):
self.ros = robot.ros
self.listener = roslibpy.Topic(self.ros, '/scan',
'sensor_msgs/msg/LaserScan')
self.listener.subscribe(self._on_msg)
self.distances = [0] * 360
self.received = False
while not self.received:
time.sleep(0.1)
def add_available_action(self, name, metadata, cls=None):
if cls:
super(ThingWrapper, self).add_available_action(name, metadata, cls)
else:
talker = roslibpy.Topic(self.client, metadata['topic'], metadata['message_type'])
self.talkers[name] = talker
super(ThingWrapper, self).add_available_action(name, metadata, PerformROSAction)
def subscribe_all(self):
for sub in self.sub_list:
if sub.topic not in self.subscribers.keys():
s = roslibpy.Topic(self.__ros, sub.topic, sub.message_type)
try:
s.subscribe(sub.sub_callback)
self.subscribers[sub.topic] = s
except:
self.__parent.print_console(
'Impossible de subscribe au topic: ' + sub.topic)
def randomNumberGenerator():
client = roslibpy.Ros(host='localhost', port=9090)
client.run()
listener = roslibpy.Topic(client, '/turtle1/pose', 'turtlesim/Pose')
listener.subscribe(lambda message: socketio.emit('newnumber', {'number': message['x']}, namespace='/test'))
try:
while not thread_stop_event.isSet():
pass
finally:
client.terminate()
def register_topic_publisher(obj, topic_name, msg_type, method_name):
topic = roslibpy.Topic(obj.ros_client, topic_name, msg_type)
pre_fn = getattr(obj, method_name)
def fn(msg):
new_msg = pre_fn(msg)
new_msg = msg if new_msg is None else new_msg
topic.publish(roslibpy.Message(new_msg))
return new_msg
setattr(obj, method_name, fn)
