def render(self, reward, state, observed_state, thrust):
if self.zcm is None:
self.zcm = ZCM("")
if self.zcm.good():
self.zcm.subscribe("target", timestamped_vector_double,
self.on_target_msg)
self.zcm.start()
if self.zcm.good():
msg = timestamped_vector_double()
msg.ts = int(self.delayed_model.time * 1e9)
def publish(channel, data):
msg.values = data
msg.len = len(data)
self.zcm.publish(channel, msg)
publish(
"quadrotor_viz/quat_pose",
np.concatenate([state.position, state.rotation.components]))
publish(
"quadrotor_viz/quat_pose_noised",
np.concatenate([
observed_state.position, observed_state.rotation.components
]))
publish("quadrotor_viz/velocity", state.velocity)
publish("quadrotor_viz/rotation_rate", state.angular_velocity)
publish("quadrotor_viz/rotatorspeed", state.propeller_speed)
publish("quadrotor_viz/control", thrust)
publish("quadrotor_viz/reward", [reward])
blddir = os.path.dirname(
os.path.realpath(__file__)) + '/../../build/examples/examples/'
sys.path.insert(0, blddir + "types/")
import time
success = "Failure"
def handler(channel, data):
global success
if data.decode('utf-8') == "test":
success = "Success"
# make a new zcm object and launch the handle thread
zcm = ZCM("")
if not zcm.good():
print("Unable to initialize zcm")
exit()
msg = "test".encode('utf-8')
# set up a subscription on channel "TEST"
subs = zcm.subscribe_raw("TEST", handler)
# publish a message
zcm.publish_raw("TEST", msg)
# wait a second
time.sleep(1)
assert (msg.orientation[0] == 1.1)
assert (msg.orientation[1] == 2.2)
assert (msg.orientation[2] == 3.3)
assert (msg.orientation[3] == 4.4)
assert (msg.num_ranges == 2)
assert (msg.num_ranges == len(msg.ranges))
assert (msg.ranges[0] == 10)
assert (msg.ranges[1] == 20)
assert (msg.name == "this is a test")
assert (msg.enabled == True)
success = "Success"
# make a new zcm object and launch the handle thread
zcm = ZCM("inproc")
if not zcm.good():
print("Unable to initialize zcm")
exit()
subs = zcm.subscribe("EXAMPLE", example_t, handler)
msg = example_t()
msg.utime = 10
msg.position = [1, 2, 3.5]
msg.orientation = [1.1, 2.2, 3.3, 4.4]
msg.ranges = [10, 20]
msg.num_ranges = len(msg.ranges)
msg.name = "this is a test"
msg.ignore0 = 0
msg.ignore1 = 0
def __init__(self):
self.transceiver = ZCM('ipc')
if not self.transceiver.good():
print("Unable to initialize ZeroCM")
exit(-1)
os.path.realpath(__file__)) + '/../../build/examples/examples/'
sys.path.insert(0, blddir + "types/")
from test_package import packaged_t
import time
success = "Failure"
def handler(channel, msg):
global success
assert (msg.a.e.timestamp == 10)
success = "Success"
# make a new zcm object and launch the handle thread
zcm = ZCM()
if not zcm.good():
print("Unable to initialize zcm")
exit()
# declare a new msg and populate it
msg = packaged_t()
msg.a.e.timestamp = 10
# set up a subscription on channel "TEST"
subs = zcm.subscribe("TEST", packaged_t, handler)
# publish a message
zcm.publish("TEST", msg)
# wait a second
