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))
def execute_move(move_msg, move_topic):
# move_msg = movements[move] # Get the ROS message for the move selected
# Execute the move
move_topic.publish(roslibpy.Message(move_msg))
time.sleep(EXECUTION_TIME)
move_topic.publish(roslibpy.Message(STOP_ROBOT))
# time.sleep(EXECUTION_TIME)
def start_talking():
while client.is_connected:
gps_talker.publish(roslibpy.Message(gps_transmit))
imu_talker.publish(roslibpy.Message(imu_transmit))
mag_talker.publish(roslibpy.Message(mag_transmit))
time.sleep(0.1)
gps_talker.unadvertise()
imu_talker.unadvertise()
mag_talker.unadvertise()
def send_control(self, control):
"""Send a control action to the robot"""
rotation = control[0]
velocity = control[1]
linear = {
'x': velocity,
'y': 0,
'z': 0
}
angular = {
'x': 0,
'y': 0,
'z': 4*rotation
}
message = roslibpy.Message({
'linear': linear,
'angular': angular,
})
def send():
print('Sending rotation: ', rotation, ' and velocity: ',velocity)
self.ros_vel.publish(message)
self.ros.on_ready(send)
def reading_task_callback(
task_idx, event_type, num_samples,
callback_data): # bufsize_callback is passed to num_samples
global daq_data
global buffer_in
if running:
# # It may be wiser to read slightly more than num_samples here, to make sure one does not miss any sample,
# # see: https://documentation.help/NI-DAQmx-Key-Concepts/contCAcqGen.html
buffer_in = np.zeros((chans_in, num_samples)) # double definition ???
reader.read_many_sample(buffer_in,
num_samples,
timeout=constants.WAIT_INFINITELY)
# talker.publish(roslibpy.Message({'data': str(buffer_in)}))
talker.publish(
roslibpy.Message({
'fx': 0,
'fy': 0,
'fz': 0,
'tx': 0,
'ty': 0,
'tz': 0
}))
# talker.publish(roslibpy.Message({'fx':'0.0','fy':'0.0','fz':'0.0','tx':'0.0','ty':'0.0','tz':'0.0'}))
# talker.publish(roslibpy.Message("['0.0', '0.0', '0.0', '0.0', '0.0', '0.0']"))
# daq_data = np.append(daq_data,buffer_in,axis=1)
# # daq_data = np.concatenate(daq_data, buffer_in, axis = 1) # appends buffered data to total variable data
print('samples:', num_samples)
return 0 # Absolutely needed for this callback to be well defined (see nidaqmx doc).
def send_and_subscribe(self, instruction, callback):
"""Send instruction and activate a service on the robot to stream feedback at a regular inverval.
Parameters
----------
instruction : :class:`compas_fab.backends.ros.messages.ROSmsg`
ROS Message representing the instruction to send.
callback
Python function to be invoked every time a new value is made available.
Notes
-----
This feature is currently only usable with custom instructions.
"""
self.ensure_protocol_version()
instruction.sequence_id = self.counter.increment()
key = _get_key(instruction)
parser = instruction.parse_feedback if hasattr(
instruction, 'parse_feedback') else None
self.futures[key] = dict(callback=callback, parser=parser)
self.topic.publish(roslibpy.Message(instruction.msg))
def __start_sending(self):
rospy.loginfo('start sending')
# send robot hearbeat msg to swarm_base robot
rate = rospy.Rate(2)
while self.__ros_client.is_connected:
try:
(trans, orient) = self.__tf_listener.lookupTransform('map', self.__robot_frame_id, rospy.Time(0))
pose_msg = {}
pose_msg['position'] = {'x':trans[0], 'y':trans[1], 'z':trans[2]}
pose_msg['orientation'] = {'x':orient[0], 'y':orient[1], 'z':orient[2], 'w':orient[3]}
heartbeat_msg = {}
heartbeat_msg['header'] = {}
heartbeat_msg['header']['stamp'] = rospy.Time.now()
heartbeat_msg['header']['frame_id'] = self.__robot_name
heartbeat_msg['state'] = self.__current_command
heartbeat_msg['pose'] = pose_msg
self.__swarm_heartbeat.publish(roslibpy.Message(heartbeat_msg))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
rospy.logwarn('tf 1error, %s' % e)
# rospy.loginfo('Swarm State: command %s', self.__current_command)
rate.sleep()
def test_land_drone(self):
# Register Drone
uid = self.register_mavros_drone("land_mavros")
# Save Topics
publishes = [{"name": "topicNameMavros", "type": "topicType"}]
service = roslibpy.Service(client, 'isaacs_server/save_drone_topics',
'isaacs_server/TypeToTopic')
request = roslibpy.ServiceRequest({"publishes": publishes, "id": uid})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
# Land Drone
action_client = roslibpy.actionlib.ActionClient(
client, "isaacs_server/control_drone",
'isaacs_server/ControlDroneAction')
goal = roslibpy.actionlib.Goal(
action_client,
roslibpy.Message({
'id': uid,
"control_task": "land_drone"
}))
goal.on('feedback', lambda f: print(f['progress']))
goal.send()
result = goal.wait(10)
self.assertTrue(result["success"])
# Shutdown Drone
self.shutdown_mavros_drone(publishes, uid)
def start_talking():
while client.is_connected:
talker.publish(roslibpy.Message(transmit))
print('Publishing Remote GPS coordinates to /mobile/fix ...')
time.sleep(1)
talker.unadvertise()
def send_msg():
global seq
seq += 1
seq += 1
header = {"frame_id": "test", "stamp": from_epoch(time.time()), "seq": seq}
tt.publish(roslibpy.Message(header))
ros.call_later(0.01, send_msg)
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 start_talking():
while client.is_connected:
talker.publish(roslibpy.Message({'data': 'Hello World!'}))
print('Sending message...')
time.sleep(1)
talker.unadvertise()
def test_upload_mission(self):
# Register Drone
uid = self.register_mavros_drone("upload_mavros")
# Save Topics
publishes = [{"name": "topicNameMavros", "type": "topicType"}]
service = roslibpy.Service(client, 'isaacs_server/save_drone_topics',
'isaacs_server/TypeToTopic')
request = roslibpy.ServiceRequest({"publishes": publishes, "id": uid})
result = wrapped_service_call(service, request)
self.assertTrue(result["success"])
# Upload_mission
action_client = roslibpy.actionlib.ActionClient(
client, "isaacs_server/upload_mission",
'isaacs_server/UploadMissionAction')
waypoints = [
navsatfix(-35.362881, 149.165222, 0),
navsatfix(-35.362881, 149.163501, 40)
]
goal = roslibpy.actionlib.Goal(
action_client, roslibpy.Message({
'id': uid,
"waypoints": waypoints
}))
goal.on('feedback', lambda f: print(f['progress']))
goal.send()
result = goal.wait(10)
self.assertTrue(result["success"])
# Shutdown Drone
self.shutdown_mavros_drone(publishes, uid)
def start_talking():
while client.is_connected:
talker.publish(roslibpy.Message({'data': 'talker from' + d[system]}))
print('Sending message...')
time.sleep(1)
talker.unadvertise()
def perform_action(self):
color = self.input['color']
print('Color changed to: ' + color)
data = {'data': json.dumps({'color': color})}
talker0.publish(roslibpy.Message(data))
self.thing.set_property('color', color)
def perform_action(self):
payload = {'data': json.dumps({'power': 'on'})}
talker0.publish(roslibpy.Message(payload))
self.thing.set_property('on', True)
print('ToglleAction performed')
def publish_current_state(self):
'''
Publish the current state estimate and covariance from the UKF. This is
a State message containing:
- Header
- PoseWithCovariance
- TwistWithCovariance
Note that a lot of these ROS message fields will be left empty, as the
1D UKF does not track information on the entire state space of the
drone.
'''
# Prepare covariance matrices
# 36-element array, in a row-major order, according to ROS msg docs
pose_cov_mat = np.full((36, ), np.nan)
twist_cov_mat = np.full((36, ), np.nan)
pose_cov_mat[14] = self.ukf.P[0, 0] # z variance
twist_cov_mat[14] = self.ukf.P[1, 1] # z velocity variance
state_msg_dict = {
'header': {
'stamp': {
'secs': self.last_time_secs,
'nsecs': self.last_time_nsecs
},
'frame_id': 'global'
},
'pose_with_covariance': {
'pose': {
'position': {
'x': np.nan,
'y': np.nan,
'z': self.ukf.x[0]
},
'orientation': {
'x': np.nan,
'y': np.nan,
'z': np.nan,
'w': np.nan
}
},
'covariance': list(pose_cov_mat)
},
'twist_with_covariance': {
'twist': {
'linear': {
'x': np.nan,
'y': np.nan,
'z': self.ukf.x[1]
},
'angular': {
'x': np.nan,
'y': np.nan,
'z': np.nan
}
},
'covariance': list(twist_cov_mat)
}
}
self.state_pub.publish(roslibpy.Message(state_msg_dict))
def disengage_safe(self):
self.command = {
"joints_mask": MaskType.JOINT_MASK7.value,
"disengage_steps": 1,
"disengage_offset": 0.0
}
self.JointReset.publish(roslibpy.Message(self.command))
if self.Verbose : print('Safe Disengage')
def disengage_std(self):
self.command = {
"joints_mask": MaskType.JOINT_MASK7.value,
"disengage_steps": 2000,
"disengage_offset": 3.5
}
self.JointReset.publish(roslibpy.Message(self.command))
if self.Verbose : print('Standard Disengage')
def init_6Axes(self):
self.command = {
"mode": 0,
"joints_mask": MaskType.JOINT_MASK6.value,
"reduction_factor": 0.0
}
self.JointInit.publish(roslibpy.Message(self.command))
if self.Verbose : print('e.DO with 6 Joints has been initialized')
def stop(self):
self.ensure_is_connected()
self.joyPublisher.publish(
roslibpy.Message({
'buttons': [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
'axes': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
}))
def on_release(key):
if str(key) == "'t'":
print("Status is", self.t_status)
self.t_status = not self.t_status
msg = {'data': int(self.t_status)}
self.talker_t.publish(roslibpy.Message(msg))
if str(key) == "'s'":
instances = get(ip_adr + ':' + port_rest + '/Instances',
log=False)
for index, ins in enumerate(instances):
if ins['comp']['pretty_name'] == 'UR5':
i = index
urdf_dyn = ins['comp']['urdf_stat']
pose = urdf_dyn.find(s_pose)
out = insert_str(urdf_dyn, pose, add_string_to_xml)
# modified URDF with tooltip
data = {'data': out}
post(
ip_adr + ':' + port_rest +
'/Instances/%d/inst/urdf_dyn' % i, data)
if str(key) == "'f'":
instances = get(ip_adr + ':' + port_rest + '/Instances',
log=False)
for index, ins in enumerate(instances):
if ins['comp']['pretty_name'] == 'UR5':
i = index
urdf_dyn = ins['comp']['urdf_stat']
pose = urdf_dyn.find(s_pose)
out = insert_str(urdf_dyn, pose, add_string_to_xml_2)
# modified URDF with tooltip
data = {'data': out}
post(
ip_adr + ':' + port_rest +
'/Instances/%d/inst/urdf_dyn' % i, data)
if str(key) == "'d'":
instances = get(ip_adr + ':' + port_rest + '/Instances')
for index, ins in enumerate(instances):
if ins['comp']['pretty_name'] == 'UR5':
i = index
urdf_stat = ins['comp']['urdf_stat']
# reset urdf_dyn to stat
data = {'data': urdf_stat}
post(
ip_adr + ':' + port_rest +
'/Instances/%d/inst/urdf_dyn' % i, data)
elif key == Key.esc:
return False
return True
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 move_cancel(self):
self.command = {**self.command_template}
self.command['move_command'] = MoveCommand.CANCEL_MOVE.value
self.command['move_type'] = MoveType.JOINT.value
self.command['target']['data_type'] = DataType.E_MOVE_POINT_JOINT.value
self.command['target']['joints_mask'] = MaskType.JOINT_MASK7.value
self.MovementCommand.publish(roslibpy.Message(self.command))
if self.Verbose : print('Move Cancel')
def parking_start(request, node_info):
node = Node.objects.all().get(node_name = node_info)
if True:
if not client.is_connected:
client.run()
goal.publish(roslibpy.Message({'data':node_info}))
#time.sleep(1)
#goal.unadvertise()
#client.close()
return render(request, 'parking_start.html', {'node':node})
def qtc_update_callback(self,qtc_state_msg):
qtc_state_str = qtc_state_msg['data'].replace(",", "")
current_time = time.time()
if current_time - self.prev_time > 3:
self.qtc_seq = []
self.sitTopic.publish(roslibpy.Message({'data': "None"}))
if len(self.qtc_seq) == 0:
self.qtc_seq.append(qtc_state_str)
elif qtc_state_str != self.qtc_seq[-1]:
self.qtc_seq.append(qtc_state_str)
#print(self.qtc_seq)
sit = self.classify_QTC_seqs(np.array([self.QTC_C_seq_to_num_seq(self.qtc_seq)]))
#print(sit, "\n")
self.sitTopic.publish(roslibpy.Message({'data': sit}))
self.prev_time = current_time
def on_message(client, userdata, message):
# TODO: define more messages. Currently for testing is only used the velocity
controller.log_debug("Received message :" + str(message.payload) +
" on topic " + message.topic + " with QoS " +
str(message.qos))
if message.topic == sinamics_topics['velocity']:
number = int.from_bytes(message.payload, 'little', signed=True)
ros_message = roslibpy.Message({'data': str(number)})
controller.mqtt_subscribers[
sinamics_topics['velocity']]['ros_topic'].publish(ros_message)
def sendAction():
global client
now_sec = client.get_time().to_sec()
action_client = roslibpy.actionlib.ActionClient(
client, '/joint_trajectory_action',
'control_msgs/FollowJointTrajectoryAction')
point_array = []
for i, joint_position in enumerate(trajectory):
v = [bpy.context.scene.controls.speed] * 6
a = [1, 1, 1, 1, 1, 1]
if (i == 0) or (i == len(trajectory) - 1) or (i == len(trajectory)):
v = [0, 0, 0, 0, 0, 0]
a = [0, 0, 0, 0, 0, 0]
point_array.append(
dict(
positions=joint_position,
velocities=v,
accelerations=a,
# effort=[],
time_from_start=roslibpy.Time(0, 500000000),
))
#print(point_array)
goal = roslibpy.actionlib.Goal(
action_client,
roslibpy.Message(
dict(
# header=roslibpy.Header(seq=1, stamp=roslibpy.Time(now_sec, 0), frame_id=''),
# goal_id= dict(stamp='', id=''), #stamp=roslibpy.Time(now_sec, 0), frame_id='holi'),
# goal=dict(
trajectory=dict(
header=roslibpy.Header(seq=1,
stamp=roslibpy.Time(now_sec, 0),
frame_id=''), #roslibpy.Time.now()
joint_names=[
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5',
'joint_6'
],
points=point_array,
),
# control_msgs/JointTolerance[] path_tolerance
# control_msgs/JointTolerance[] goal_tolerance
#goal_time_tolerance = roslibpy.Time(100, 0),
# ),
)))
goal.on('feedback', lambda f: print(f['sequence']))
goal.on('result', lambda f: print(f['sequence']))
goal.send()
#result = goal.wait(30)
action_client.dispose()
def on_file(self, widget):
dlg = Gtk.FileChooserDialog("Open..", None, Gtk.FileChooserAction.OPEN,
(Gtk.STOCK_CANCEL, Gtk.ResponseType.CANCEL,
Gtk.STOCK_OPEN, Gtk.ResponseType.OK))
response = dlg.run()
if response == Gtk.ResponseType.OK:
self.main_file_publisher.publish(
roslibpy.Message({'data': dlg.get_filename()}))
self.builder.get_object('choosed-file').set_text(
dlg.get_filename())
dlg.destroy()
def drive_forward(self, duration: float, power: float):
self.ensure_is_connected()
power = self.normalize_power(power)
self.joyPublisher.publish(
roslibpy.Message({
'buttons': [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
'axes': [0.0, power, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
}))
time.sleep(duration)
self.stop()
