def parse_data_strm(self, data):
if self.is_connected:
now = rospy.Time.now()
imu = Imu(header=rospy.Header(frame_id="imu_link"))
imu.header.stamp = now
imu.orientation.x = data["QUATERNION_Q0"]
imu.orientation.y = data["QUATERNION_Q1"]
imu.orientation.z = data["QUATERNION_Q2"]
imu.orientation.w = data["QUATERNION_Q3"]
imu.linear_acceleration.x = data["ACCEL_X_FILTERED"]/4096.0*9.8
imu.linear_acceleration.y = data["ACCEL_Y_FILTERED"]/4096.0*9.8
imu.linear_acceleration.z = data["ACCEL_Z_FILTERED"]/4096.0*9.8
imu.angular_velocity.x = data["GYRO_X_FILTERED"]*10*math.pi/180
imu.angular_velocity.y = data["GYRO_Y_FILTERED"]*10*math.pi/180
imu.angular_velocity.z = data["GYRO_Z_FILTERED"]*10*math.pi/180
self.imu = imu
self.imu_pub.publish(self.imu)
odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_footprint')
odom.header.stamp = now
odom.pose.pose = Pose(Point(data["ODOM_X"]/100.0,data["ODOM_Y"]/100.0,0.0), Quaternion(0.0,0.0,0.0,1.0))
odom.twist.twist = Twist(Vector3(data["VELOCITY_X"]/1000.0, 0, 0), Vector3(0, 0, data["GYRO_Z_FILTERED"]*10.0*math.pi/180.0))
odom.pose.covariance =self.ODOM_POSE_COVARIANCE
odom.twist.covariance =self.ODOM_TWIST_COVARIANCE
self.odom_pub.publish(odom)
#need to publish this trasform to show the roll, pitch, and yaw properly
self.transform_broadcaster.sendTransform((0.0, 0.0, 0.038 ),
(data["QUATERNION_Q0"], data["QUATERNION_Q1"], data["QUATERNION_Q2"], data["QUATERNION_Q3"]),
odom.header.stamp, "base_link", "base_footprint")
def emit_propagation(self, w: GeoData, w_timestamp: datetime.datetime, p: GeoData,
p_timestamp: datetime.datetime,
until: datetime.datetime):
"""Publish current situation assessment."""
current = None
if w:
current = WildfireMap(
header=rospy.Header(stamp=rospy.Time.from_sec(w_timestamp.timestamp())),
raster=serialization.raster_msg_from_geodata(w, 'ignition'))
pred = PredictedWildfireMap(
header=rospy.Header(stamp=rospy.Time.from_sec(p_timestamp.timestamp())),
last_valid=rospy.Time.from_sec(until.timestamp()),
raster=serialization.raster_msg_from_geodata(p, 'ignition'))
# self.pub_wildfire_real.publish(pred)
self.pub_wildfire_pred.publish(pred)
# self.pub_wildfire_real.publish(WildfireMap(
# header=rospy.Header(stamp=rospy.Time.from_sec(p_timestamp.timestamp())),
# raster=serialization.raster_msg_from_geodata(p, 'ignition')))
if current:
self.pub_wildfire_current.publish(current)
if self.sa.elevation_timestamp > self.last_elevation_timestamp:
# pub_elevation is latching, don't bother other nodes with unnecessary elevation updates
elevation = ElevationMap(
header=rospy.Header(
stamp=rospy.Time.from_sec(self.sa.elevation_timestamp.timestamp())),
raster=serialization.raster_msg_from_geodata(self.sa.elevation, 'elevation'))
self.pub_elevation.publish(elevation)
rospy.loginfo("A new Elevation Map has been generated")
self.last_elevation_timestamp = self.sa.elevation_timestamp
g = GeoDataDisplay.pyplot_figure(p)
g.draw_ignition_shade(with_colorbar=True)
g.figure.savefig("/home/rbailonr/fuego_pre.png")
g.close()
if w:
g = GeoDataDisplay.pyplot_figure(w)
g.draw_ignition_shade(with_colorbar=True)
g.figure.savefig("/home/rbailonr/fuego_cur.png")
g.close()
g = GeoDataDisplay.pyplot_figure(self.sa.observed_wildfire.geodata)
g.draw_ignition_shade(with_colorbar=True)
g.figure.savefig("/home/rbailonr/fuego_obs.png")
# obs_cpp = self.sa.observed_wildfire.geodata.as_cpp_raster("ignition")
# obs_cpp_bytes = obs_cpp.encoded(3035)
# print("Compressed: ")
# print(obs_cpp_bytes.hex())
# obs_cpp_bytes = obs_cpp.encoded_uncompressed(3035)
# print("Uncompressed: ")
# print(obs_cpp_bytes.hex())
g.close()
del g
rospy.loginfo("Wildfire propagation publishing ended")
def random_sonar(delay=1):
print('Starting random sonar...')
pub = Publisher('/sensors/range', Range)
msg = Range()
while not rospy.is_shutdown():
msg.header = rospy.Header(frame_id='right_sonar_frame')
msg.range = random.random() * 20
pub.publish(msg)
sleep(delay)
msg.header = rospy.Header(frame_id='left_sonar_frame')
msg.range = random.random() * 20
pub.publish(msg)
def pub_alarm(posx, posy):
start_wind = (3.0, 3 * np.pi / 2)
environment = fire_rs.firemodel.propagation.Environment(
area, start_wind[0], start_wind[1], fire_rs.geodata.environment.World(
**world_paths,
landcover_to_fuel_remap=fire_rs.geodata.environment.EVERYTHING_FUELMODEL_REMAP))
rw = fire_rs.simulation.wildfire.RealWildfire(
datetime.datetime.fromtimestamp(
(rospy.Time.now() - rospy.Duration.from_sec(10 * 60)).to_sec()),
environment)
ignitions = [(posx, posy), ]
ignitions_cell = [rw.fire_map.array_index(p) for p in ignitions]
rw.ignite((posx, posy))
rospy.loginfo("ignite %s", str((posx, posy)))
rw.propagate(datetime.timedelta(minutes=90.))
rospy.loginfo("propagate 90 min")
wind = MeanWindStamped(header=Header(stamp=rospy.Time.now()), speed=start_wind[0],
direction=start_wind[1])
wind_pub.publish(wind)
rospy.loginfo("Notify wind")
rospy.loginfo(wind)
firemap = rw.perimeter(rospy.Time.now().to_sec()).geodata
wildfire_message = WildfireMap(header=rospy.Header(stamp=rospy.Time.now()),
raster=serialization.raster_msg_from_geodata(
firemap,
layer="ignition"))
map_pub.publish(wildfire_message)
wildfire_real_message = WildfireMap(header=rospy.Header(stamp=rospy.Time.now()),
raster=serialization.raster_msg_from_geodata(
rw.fire_map,
layer="ignition"))
rospy.loginfo("Notify alarm map at 15 min")
rospy.loginfo(wildfire_message)
pub_wildfire_real.publish(wildfire_real_message)
rospy.loginfo(wildfire_real_message)
firemap.data["ignition"][firemap.data["ignition"] == np.inf] = 0
firemap.data["ignition"][firemap.data["ignition"].nonzero()] = 65535
firemap.write_to_image_file("/home/rbailonr/fire.png", "ignition")
fire_pos = Timed2DPointStamped(header=Header(stamp=rospy.Time.now()),
x=posx, y=posy,
t=rospy.Time.now() - rospy.Duration.from_sec(60 * 30))
alarm_pub.publish(fire_pos)
rospy.loginfo("Notify alarm point")
rospy.loginfo(fire_pos)
def parse_data_strm(self, data):
"""
Unpack streaming data from accelometer, gyroscope and odometer.
"""
if self.is_connected:
now = rospy.Time.now()
imu = Imu(header=rospy.Header(frame_id="imu_link"))
imu.header.stamp = now
imu.orientation.x = data["QUATERNION_Q0"]
imu.orientation.y = data["QUATERNION_Q1"]
imu.orientation.z = data["QUATERNION_Q2"]
imu.orientation.w = data["QUATERNION_Q3"]
imu.linear_acceleration.x = data["ACCEL_X_FILTERED"] / 4096.0 * 9.8
imu.linear_acceleration.y = data["ACCEL_Y_FILTERED"] / 4096.0 * 9.8
imu.linear_acceleration.z = data["ACCEL_Z_FILTERED"] / 4096.0 * 9.8
imu.angular_velocity.x = data[
"GYRO_X_FILTERED"] * 10 * math.pi / 180
imu.angular_velocity.y = data[
"GYRO_Y_FILTERED"] * 10 * math.pi / 180
imu.angular_velocity.z = data[
"GYRO_Z_FILTERED"] * 10 * math.pi / 180
imu.orientation_covariance = IMU_ORIENTATION_COVARIANCE
imu.angular_velocity_covariance = IMU_ANG_VEL_COVARIANCE
imu.linear_acceleration_covariance = IMU_LIN_ACC_COVARIANCE
self.imu_pub.publish(imu)
odom = Odometry(header=rospy.Header(frame_id="odom"),
child_frame_id='base_footprint')
odom.header.stamp = now
odom.pose.pose = Pose(Point(data["ODOM_X"], data["ODOM_Y"], 0.0),
Quaternion(0.0, 0.0, 0.0, 1.0))
odom.twist.twist = Twist(
Vector3(data["VELOCITY_X"], data["VELOCITY_Y"], 0),
Vector3(0, 0, 0))
# Save current x and y position.
self.odom.pose.pose.position.x = odom.pose.pose.position.x
self.odom.pose.pose.position.y = odom.pose.pose.position.y
odom.pose.covariance = ODOM_POSE_COVARIANCE
odom.twist.covariance = ODOM_TWIST_COVARIANCE
# This enables resetting odometry data.
odom.pose.pose.position.x -= self.odom_tare.pose.pose.position.x
odom.pose.pose.position.y -= self.odom_tare.pose.pose.position.y
self.odom_pub.publish(odom)
def bb_est_feedback(feedback):
global object_added
rospy.loginfo('Feedback received')
print feedback
est = rospy.ServiceProxy('/bb_estimator/estimate_bb_alt', EstimateBBAlt)
header = rospy.Header()
header.frame_id = '/map'
header.stamp = feedback.timestamp
rospy.loginfo('Calling bb est srv')
try:
est_result = est(header=header, p1=feedback.p1, p2=feedback.p2, mode=1)
except Exception, e:
rospy.logerr("Error on calling service: %s", str(e))
return
def train(word):
global chatsubscribe
global listened
global client_lcd
newsentence = 1
while newsentence == 1:
client_speak(
"Now I will say to you some random words and you have to repeat them"
)
client_speak("the words also will be displayed in the LCD")
words = gengrammar.getRandomwords(
"/opt/qbo/ros_stacks/qbo_apps/qbo_listen/config/AM/en/phonems", 2)
text = ""
for i in range(0, 2):
text = text + " " + words[i]
text = text + " " + word
words = gengrammar.getRandomwords(
"/opt/qbo/ros_stacks/qbo_apps/qbo_listen/config/AM/en/phonems", 2)
for i in range(0, 2):
text = text + " " + words[i]
rospy.loginfo(text)
client_lcd.publish(
LCD(rospy.Header(None, None, "frame"),
chr(12) + text))
client_speak(text)
client_speak("Do you want to change the words")
if waitaccept() == 1:
continue
else:
newsentence = 0
return text
def handle_freetrain(req):
# return 0
# global activado
#print fileLocation
global client_lcd
response = 0
while response == 0:
juliusServer = pyJulius.JuliusServer("./configbis/julius.jconf")
time.sleep(2)
pattern = re.compile('\s?sentence1\:\s+(?P<word>(\w+ )+)\s?')
# pattern = re.compile ('\s?sentence1\:\s+\<s\>\s+(?P<word>(\w+ )+)\s?\<.s\>')
# pattern = re.compile ('\s?sentence1\:\s+(?P<word>(\w+ )+)\s?')
print "inicio reconocedor"
while not rospy.is_shutdown():
juliusOutput = juliusServer._process.stdout.readline()
#print juliusOutput
result = pattern.match(juliusOutput)
if result:
print "RECONOCIDO"
text = result.group('word')
#pub.publish(text.strip())
print text
juliusServer.stop()
client_lcd.publish(
LCD(rospy.Header(None, None, "frame"),
chr(12) + text))
move_180()
break
response = waitaccept()
move_180()
return 1
def call_execute_cartesian_ik_trajectory(frame, positions, orientations):
rospy.wait_for_service("execute_cartesian_ik_trajectory")
#fill in the header (don't need seq)
header = rospy.Header()
header.frame_id = frame
header.stamp = rospy.get_rostime()
#fill in the poses
poses = []
for (position, orientation) in zip(positions, orientations):
pose = Pose()
pose.position.x = position[0]
pose.position.y = position[1]
pose.position.z = position[2]
pose.orientation.x = orientation[0]
pose.orientation.y = orientation[1]
pose.orientation.z = orientation[2]
pose.orientation.w = orientation[3]
poses.append(pose)
#call the service to execute the trajectory
print "calling execute_cartesian_ik_trajectory"
try:
s = rospy.ServiceProxy("execute_cartesian_ik_trajectory", \
ExecuteCartesianIKTrajectory)
resp = s(header, poses)
except rospy.ServiceException, e:
print "error when calling execute_cartesian_ik_trajectory: %s" % e
return 0
def callback(self, rgb_msg):
rgb_img = self.bridge.imgmsg_to_cv2(rgb_msg, desired_encoding="bgr8")
rgb_img = cv2.cvtColor(rgb_img, cv2.COLOR_BGR2RGB)
ltop_point, rbottom_point = self.detect_table(rgb_img.copy())
table_img = rgb_img.copy()[ltop_point[1]:rbottom_point[1], ltop_point[0]:rbottom_point[0], :]
box_ltop, box_rbottom = self.detect_box(table_img)
box_ltop = box_ltop+ltop_point
box_rbottom = box_rbottom + ltop_point
box_center = (box_ltop+box_rbottom)/2.0
box_center = box_center.astype(np.int)
cv2.rectangle(rgb_img, tuple(ltop_point), tuple(rbottom_point), color=(0,0,255), thickness=2)
cv2.rectangle(rgb_img, tuple(box_ltop), tuple(box_rbottom), color=(255, 0, 0), thickness=2)
cv2.circle(rgb_img, (box_center[0], box_center[1]), 2, (255, 0, 0))
cv2.imshow('vision', rgb_img)
cv2.waitKey(1)
obj_camera_frame = PointStamped(
header=rospy.Header(stamp = rospy.get_rostime(), frame_id="/camera_link"),
point=Point(
x=0.45,
y=-box_center[1],
z=-box_center[0]))
obj_robot_frame = self.tf_listener.transformPoint(target_frame="/base_link", ps=obj_camera_frame)
print("obj_robot_frame_tf", obj_robot_frame)
self.box_center_x = obj_robot_frame.point.x
self.box_center_y = obj_robot_frame.point.y
self.box_center_z = obj_robot_frame.point.z
def test_roscore__monitored_dl_watertank_safe_after_unsafe(
simple_pipeline, launch_monitor):
h = ros.Header()
h.stamp = ros.Time.now()
launch_monitor('monitor-dl-watertank')
sensor_callback = CallBack(Float32Packet.to_formatted_string)
sensor_packets = [
Float32Packet('/level_sensor', [x], sensor_callback)
for x in [0.0, 0.0, 0.0]
]
control_callback = CallBack(Float32StampedPacket.to_formatted_string)
control_packets = [
Float32StampedPacket('/flow_control_cmd', [Float32Stamped(h, x)],
control_callback) for x in [0.0, 0.5, 0.1]
]
simple_pipeline([
sensor_packets[0], control_packets[0], sensor_packets[1],
control_packets[1], sensor_packets[2], control_packets[2]
])
rate = ros.Rate(10)
rate.sleep()
rate.sleep()
rate.sleep()
rate.sleep()
# Unsafe Control to safe (fallback) control and safe (blackbox) control
assert (['0.0', '0.0', '0.1'] == control_callback.recieved_list)
# Sensor Readings Unchanged
assert (['0.0', '0.0', '0.0'] == sensor_callback.recieved_list)
def move(self, p, v_scale=0.1, duration_low_bound=1, start_duration=0.5):
target_q = self.get_inverse_kin(p)
if target_q is None:
return False
traj = JointTrajectory()
traj.header = rospy.Header(frame_id="1", stamp=rospy.Time())
traj.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
dist = np.abs(np.subtract(target_q,
self.current_state.positions)).max()
duration = rospy.Duration(
nsecs=int(max(dist / v_scale, duration_low_bound) * 1e9))
# print(dist,duration.secs,duration.nsecs)
end_point = JointTrajectoryPoint(
positions=target_q,
velocities=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
time_from_start=duration)
start_point = JointTrajectoryPoint(
positions=(np.array(self.current_state.positions) +
np.array(self.current_state.velocities) *
(start_duration + 0.01)).tolist(),
velocities=self.current_state.velocities,
time_from_start=rospy.Duration(0, int(start_duration * 1e9)))
traj.points = [start_point, end_point]
self.pub.publish(traj)
return True
def propagate_and_set_fire_front(
self, origin, origin_time: rospy.Time, perimeter_time: rospy.Time,
environment: fire_rs.firemodel.propagation.Environment):
origin_pos = Timed2DPointStamped(header=Header(stamp=rospy.Time.now()),
x=origin[0],
y=origin[1],
t=origin_time)
fireprop = fire_rs.firemodel.propagation.propagate_from_points(
environment,
fire_rs.firemodel.propagation.TimedPoint(*origin,
origin_time.to_sec()))
firemap = fireprop.ignitions()
array, cells, contours = fire_rs.geodata.wildfire._compute_perimeter(
firemap, perimeter_time.to_sec())
# Publish ignition points
# initial alarm
self.pub_w.publish(origin_pos)
self.pub_w.publish(origin_pos)
rospy.loginfo(origin_pos)
wildfire_message = WildfireMap(
header=rospy.Header(stamp=rospy.Time.now()),
raster=serialization.raster_msg_from_geodata(
firemap.clone(data_array=array, dtype=firemap.data.dtype),
'ignition'))
self.pub_wildfire_obs.publish(wildfire_message)
rospy.loginfo(wildfire_message)
def __init__(self):
print "Initializing"
self._Counter = 0
self._ax = 0
port = rospy.get_param("~port", "/dev/gyro")
baudRate = int(rospy.get_param("~baudRate", 115200))
rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baudRate))
self._BananaSerial = BananaSerial(port, baudRate, self._HandleReceivedLine)
rospy.loginfo("Started serial communication")
#Publisher for entire serial data
self._SerialPublisher = rospy.Publisher('serial', String,queue_size=10)
#Subscribers and Publishers of IMU data topic
self.frame_id = '/IMU_link'
self.cal_buffer =[]
self.cal_buffer_length = 1000
self.imu_data = Imu(header=rospy.Header(frame_id="IMU_link"))
self.imu_data.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
self.imu_data.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e6]
self.imu_data.linear_acceleration_covariance = [-1,0,0,0,0,0,0,0,0]
self.yaw_pub = rospy.Publisher('yaw_from_imu', Float64, queue_size = 10)
self.imu_pub = rospy.Publisher('imu_data', Imu, queue_size = 10)
def set_goal_handler(self, path, qdict): pose = get_pose(self, qdict) h = rospy.Header() h.stamp= rospy.get_rostime() h.frame_id = '/map' goal = PoseStamped(h, pose) goal_publisher.publish(goal) self.send_success()
def get_tf(self):
self.h = rospy.Header(frame_id=self.fixed_frame)
self.tf_matrix = self.tfListener.asMatrix(self.target_frame, self.h)
# Alternate form
self.tf_translation = tf.transformations.translation_from_matrix(
self.tf_matrix)
self.tf_quaternion = tf.transformations.quaternion_from_matrix(
self.tf_matrix)
def QtPolyToROS(poly, name, x, y, z, res, frame_id):
header = rospy.Header()
header.frame_id = frame_id
ps = PolygonStamped(header=header)
for pt in poly:
# Qt defines 0,0 in the upper left, so, and down as +y, so flip the y
ps.polygon.points.append(Point(pt.x() * res + x, -pt.y() * res + y, z))
return ps
def parse_data_strm(data):
odom_pub = rospy.Publisher(name + '/odom', Odometry, queue_size=1)
imu_pub = rospy.Publisher(name + '/imu', Imu, queue_size=1)
ODOM_POSE_COVARIANCE = [
1e-3, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0,
1e6, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e3
]
ODOM_TWIST_COVARIANCE = [
1e-3, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0,
1e6, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e3
]
#if is_connected:
now = rospy.Time.now()
imu = Imu(header=rospy.Header(frame_id="imu_link"))
imu.header.stamp = now
imu.orientation.x = data["QUATERNION_Q0"]
imu.orientation.y = data["QUATERNION_Q1"]
imu.orientation.z = data["QUATERNION_Q2"]
imu.orientation.w = data["QUATERNION_Q3"]
imu.linear_acceleration.x = data["ACCEL_X_FILTERED"] / 4096.0 * 9.8
imu.linear_acceleration.y = data["ACCEL_Y_FILTERED"] / 4096.0 * 9.8
imu.linear_acceleration.z = data["ACCEL_Z_FILTERED"] / 4096.0 * 9.8
imu.angular_velocity.x = data["GYRO_X_FILTERED"] * 10 * math.pi / 180
imu.angular_velocity.y = data["GYRO_Y_FILTERED"] * 10 * math.pi / 180
imu.angular_velocity.z = data["GYRO_Z_FILTERED"] * 10 * math.pi / 180
imu_pub.publish(imu)
odom = Odometry(header=rospy.Header(frame_id="odom"),
child_frame_id='base_footprint')
odom.header.stamp = now
odom.pose.pose = Pose(
Point(data["ODOM_X"] / 100.0, data["ODOM_Y"] / 100.0, 0.0),
Quaternion(0.0, 0.0, 0.0, 1.0))
odom.twist.twist = Twist(
Vector3(data["VELOCITY_X"] / 1000.0, 0, 0),
Vector3(0, 0, data["GYRO_Z_FILTERED"] * 10.0 * math.pi / 180.0))
odom.pose.covariance = ODOM_POSE_COVARIANCE
odom.twist.covariance = ODOM_TWIST_COVARIANCE
odom_pub.publish(odom)
rospy.loginfo(odom.pose.pose)
def mouseMoveEvent(self, e):
pos = np.array(e.pos().toTuple())
pos[1] = self.size().height() - pos[1] # make +y up
pos_world = pos * self.scale + self.offset
cloud = create_cloud_xyz32(
rospy.Header(frame_id=self.frame_id, stamp=rospy.Time.now()),
np.array([[pos_world[0], pos_world[1], 0]]))
self.point_pub.publish(cloud)
def point_head_cart_client(x, y, z, frame):
head_angles = rospy.Publisher('head_controller/point_head', PointStamped)
rospy.init_node('head_commander', anonymous=True)
sleep(1)
head_angles.publish(
PointStamped(rospy.Header(None, rospy.get_rostime(), frame),
Point(x, y, z)))
sleep(1)
def test_moveit(self):
p = [0.35, -0.35, 0.4]
pose = PoseStamped(
header=rospy.Header(stamp=rospy.Time.now(), frame_id='/BASE'),
pose=Pose(position=Point(*p),
orientation=Quaternion(
*quaternion_from_euler(1.57, 0, 1.57, 'sxyz'))))
self.arm.set_pose_target(pose)
self.assertTrue(self.arm.go() or self.arm.go())
def spin(self):
encoders = [0, 0]
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
then = rospy.Time.now()
# things that don't ever change
scan_link = rospy.get_param('~frame_id', 'neato_link')
scan = LaserScan(header=rospy.Header(frame_id=scan_link))
scan.angle_min = 0
scan.angle_max = 6.26
scan.angle_increment = 0.017437326
scan.range_min = 0.020
scan.range_max = 5.0
# The LiDAR spins counterclockwise at 10 revolutions per second.
# Each revolution yields 90 packets.
# Each packet contains 22 bytes.
# Within these 22 bytes are 4 distance measurements and more
# (4 data points/packet * 90 packets = 360 data points).
# So there is one data measurement per degree turn.
# Byte 01, "FA" is a starting byte which appears between the ending
# and beginning of two packets.
# Byte 02 is a hex value from A0-F9, representing the 90 packets
# outputted per revolution.
# Byte 03 and 04 are a 16bit (combined) value representing the speed
# at which the LiDAR is rotating.
# Bytes 06 and 05 are 1st distance measurement in this packet.
# Bytes 10 and 09 are 2nd distance measurement in this packet.
# Bytes 14 and 13 are 3rd distance measurement in this packet.
# Bytes 18 and 17 are the 4th distance measurement in this packet.
# Bytes 08:07, 12:11, 16:15, and 20:19 represent information about
# the surface off of which the laser has bounced to be detected by
# the LiDAR.
# Bytes 22 and 21 are checksum and are used for determining the
# validity of the received packet.
# main loop of driver
# r = rospy.Rate(10)
rospy.loginfo("0")
# requestScan()
data = []
i = 0
while not rospy.is_shutdown():
# string = self.port.readline()
byte = self.port.read()
b = ord(byte)
data.append(b)
i = i + 1
if i > 1000:
for j in range(0, 999):
rospy.loginfo("%d", j)
rospy.loginfo(": {:02X}".format(data[j]))
i = 0
data = []
def publishTagPos(self, x, y):
now = rospy.Time.now()
header = rospy.Header(seq=self.seq, stamp=now, frame_id=topic)
self.seq += 1
# The position is encoded here.
position = Point(x=x, y=y, z=0)
# We have no orientation information with UWB ranging.
orientation = Quaternion(x=0, y=0, z=0, w=1)
pose = Pose(position=position, orientation=orientation)
# Row-major representation of the 6x6 covariance matrix
# The orientation parameters use a fixed-axis representation.
# In order, the parameters are:
# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z
# axis)
covariance = [
0,
] * 36
# Variances for X, Y position.
covariance[idx6x6(0, 0)] = pos_var
covariance[idx6x6(1, 1)] = pos_var
# Our z, roll and pitch should be constant 0 (it's a car on flat ground).
covariance[idx6x6(2, 2)] = known
covariance[idx6x6(3, 3)] = known
covariance[idx6x6(4, 4)] = known
# We don't know the yaw. Might be anything.
covariance[idx6x6(5, 5)] = unknown
pose_w_cov = PoseWithCovariance(pose=pose, covariance=covariance)
# Fill the Twist, but we actually have no velocity information. That's thus
# all zeros.
zero = Vector3(0, 0, 0)
twist = Twist(zero, zero)
covariance = [
0,
] * 36
# X linear twist variance might be anything.
# FIXME: we could have a more fine-tuned value by taking max linear
# velocity into consideration.
covariance[idx6x6(0, 0)] = unknown
# Y linear twist variance
covariance[idx6x6(1, 1)] = known
# Z linear twist variance
covariance[idx6x6(2, 2)] = known
# X angular twist variance
covariance[idx6x6(3, 3)] = known
# Y angular twist variance
covariance[idx6x6(4, 4)] = known
# Z angular twist variance. Might be anything.
# FIXME: we could have a more fine-tuned value by taking max rotational
# velocity into consideration.
covariance[idx6x6(5, 5)] = unknown
twist_w_cov = TwistWithCovariance(twist=twist, covariance=covariance)
self.pub.publish(header=header,
child_frame_id=child_frame_id,
pose=pose_w_cov,
twist=twist_w_cov)
def random_thermal(delay=1):
print('Starting random thermal data...')
pub = Publisher('/sensors/thermal', ThermalMeanMsg)
msg = ThermalMeanMsg()
while not rospy.is_shutdown():
msg.header = rospy.Header()
msg.thermal_mean = random.randint(20, 40)
sleep(delay)
pub.publish(msg)
def random_co2(delay=1):
print('Starting random battery data...')
pub = Publisher('/sensors/co2', Co2Msg)
msg = Co2Msg()
while not rospy.is_shutdown():
msg.header = rospy.Header()
msg.co2_percentage = random.random() * 10
sleep(delay)
pub.publish(msg)
def get_observation_info(self):
"""
Generate a resource information for publishing by readout the filter and
make hypotheses checks.
:return a resource information that can be published
"""
def get_valid_states(value, deviation, sample_size):
"""
Check hypotheses of all states.
:param value: mean value which should be checked
:param deviation: deviation values which should be checked
:param sample_size: number of samples that are used for mean and deviation
"""
observations = []
for state in self._states:
try:
if state.check_hypothesis(value, deviation, sample_size):
observations.append(
observation(observation_msg=state.name,
verbose_observation_msg=state.name,
observation=state.number))
except AttributeError as e:
rospy.logerr(e)
if not observations:
observations.append(observation_no_state_fits)
return observations
mean, deviation, sample_size = self._filter.get_value()
# check if filter contains usable information
if mean is None:
self._observation_info.observation = [observation_no_state_fits]
self._observation_info.header = rospy.Header(
stamp=rospy.Time.now())
return self._observation_info
# setup predefined resource information
self._observation_info.observation = get_valid_states(
mean, deviation, sample_size)
self._observation_info.header = rospy.Header(stamp=rospy.Time.now())
return self._observation_info
def test_futureSim(passed_roomba_state,passed_actions):
# Create a PoseWithCovariance
header = rospy.Header(stamp=rospy.Time(secs=2))
q = tf.transformations.quaternion_from_euler(0, 0, math.pi)
msg = Pose(position=Point(0, 0, 0), orientation=Quaternion(q[0], q[1], q[2], q[3]))
pose = PoseWithCovariance(pose=msg)
start_pose = PoseWithCovarianceStamped(header=header, pose=pose)
roomba_pos = futureSim(passed_roomba_state, passed_actions, start_pose)
return
def sendMessage(self):
msg = MedBotSpeechStatus()
msg.header = rospy.Header()
msg.header.stamp = rospy.Time.now()
msg.weight = self.weight
msg.name = self.label
msg.status = self.status.value
msg.speaking = self.speaking
msg.azimuth = self.azimuth
self.pub.publish(msg)
def labeled_cloud_pub_client(cloud, frame):
rospy.wait_for_service('pub_cloud_service')
try:
service = rospy.ServiceProxy('pub_cloud_service', CloudPub)
cloud.header = rospy.Header(frame_id=frame)
resp = service(cloud, frame)
return resp.success #bool
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def set_pose_handler(self, path, qdict): pose = get_pose(self, qdict) h = rospy.Header() h.stamp= rospy.get_rostime() h.frame_id = '/map' cov = [float64(0)] * 36 cov[6*0+0] = 0.5 * 0.5; cov[6*1+1] = 0.5 * 0.5; cov[6*3+3] = math.pi/12.0 * math.pi/12.0; pose_publisher.publish(PoseWithCovarianceStamped(h, PoseWithCovariance(pose, cov))) self.send_success()
