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 test_map_received(self):
poseMsg = PoseStamped()
poseMsg.header = Header()
poseMsg.pose.position.x = 0
poseMsg.pose.position.y = 0
poseMsg.pose.position.z = 0
poseMsg.pose.orientation.x = 0
poseMsg.pose.orientation.y = 0
poseMsg.pose.orientation.z = 0
poseMsg.pose.orientation.w = 1
self.pose_pub.publish(poseMsg)
rangeMsg = RangeArray()
rangeMsg.header = Header()
ranges = [0, 0.2, -1, 1, 1.5, 8, 3, 1, 8, 4, 2.3, 0.2, 0.1, 2, 3, 1, 9]
for rangerange in ranges:
range = Range()
range.header = Header()
range.range = rangerange
range.field_of_view = 1
range.min_range = 0.1
range.max_range = 2.1
rangeMsg.ranges.append(range)
rangeMsg.ranges = rangeMsg.ranges[:self.NUM_RANGE_SENSORS]
self.range_pub.publish(rangeMsg)
rospy.sleep(0.2)
self.assertEqual(self.rx.receivedMsg, True)
self.assertGreater(len(self.rx.map.points), 0)
def out_message(self):
msg = Range()
msg.max_range = self.max_range
msg.min_range = self.min_range
msg.range = self.return_median()
msg.header = self.header
self.publisher.publish(msg)
print "Last four ranges recived: ", np.around(self.ranges, 4), "\t\t\t\r"
def onLidarLiteUpdate(self, msg):
inMsg = msg
outMsg = Range()
outMsg.header = inMsg.header
outMsg.min_range = inMsg.range_min
outMsg.max_range = inMsg.range_max
outMsg.range = msg.ranges[1]
self.LidarLitePub.publish(outMsg)
async def publish_data(self, data):
# Although the initialization of this Range message
# including some of the values could be placed in the
# constructor for efficiency reasons. This does
# for some reason not work though.
range = Range()
range.radiation_type = range.ULTRASOUND
range.field_of_view = math.pi * 5
range.min_range = 0.02
range.max_range = 1.5
range.header = self.get_header()
range.range = data[2]
await self.publish(range)
def lidar_callback(data):
rg = Range()
rg.header = Header()
rg.radiation_type = Range().ULTRASOUND
rg.min_range = 0.00 # 2cm
rg.max_range = 13.0 # 2m
for i in range(4):
rg.header.frame_id = frame[i]
rg.header.stamp = rospy.Time.now()
rg.field_of_view = (20.0 / 180.0) * 3.14
rg.range = data.ranges[90 * i]
pub[i].publish(rg)
def default(self, ci='unused'):
msg = Range()
msg.header = self.get_ros_header()
msg.radiation_type = Range.INFRARED
msg.field_of_view = 20
msg.min_range = 0
msg.max_range = self.component_instance.bge_object['laser_range']
tmp = msg.max_range
for ray in self.data['range_list']:
if tmp > ray:
tmp = ray
msg.range = tmp
self.publish(msg)
def default(self, ci='unused'):
msg = Range()
msg.header = self.get_ros_header()
try:
msg.header.frame_id = self.kwargs['frame']
except KeyError:
msg.header.frame_id = ''
msg.radiation_type = Range.INFRARED
msg.min_range = self.component_instance.bge_object['min_range']
msg.max_range = self.component_instance.bge_object['max_range']
msg.range = self.data['range']
self.publish(msg)
def to_msg(self, sensor_id, distance):
msg = Range()
msg.header = Header()
msg.header.stamp = self.get_clock().now().to_msg()
# need to define transforms for all sensors
msg.header.frame_id = f"ultrasonic_sensor_{sensor_id}"
msg.radiation_type = 0
msg.field_of_view = 0.0
msg.min_range = self.min_range
msg.max_range = self.max_range
msg.range = distance
return msg
def handle_estimate_range(self, req):
ret = estimate_rangeResponse()
if self.estimated_distance == None:
ret.detected = False
else:
ret.detected = True
current_estimated_range = Range()
if (self.estimated_distance == None):
self.estimated_distance = 0
else:
current_estimated_range.range = self.estimated_distance
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
current_estimated_range.header = h
self.sampled_range_pub.publish(current_estimated_range)
ret.range = current_estimated_range
return ret
def callback_relay(in_msg):
global pub
# find the main axis projection of each range, and save the minimum
ang = in_msg.angle_min
inc = in_msg.angle_increment
min_range = math.inf
for range in in_msg.ranges:
val = math.cos(ang) * range
ang = ang + inc
if val < min_range:
min_range = val
# create and publish the message with noise
msg = Range()
msg.header = in_msg.header
msg.radiation_type = msg.INFRARED
msg.field_of_view = in_msg.angle_max - in_msg.angle_min
msg.min_range = in_msg.range_min
msg.max_range = in_msg.range_max
msg.range = min_range
pub.publish(msg)
def publishRangeData(self, sensor, byte1, byte2, dataLength, publisher):
if len(self.byte) == dataLength:
# Get data (decimal) from data bytes
raw = int.from_bytes(self.byte[byte1] + self.byte[byte2],
"big",
signed=False)
# Populate ROS message
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = sensor
rangeMsg = Range()
rangeMsg.header = h
rangeMsg.radiation_type = 1
rangeMsg.field_of_view = 0.3
rangeMsg.min_range = 0.0
rangeMsg.max_range = 0.2
rangeMsg.range = (float(700 - raw) / 700.0) * 0.2
# Publish data
publisher.publish(rangeMsg)
else:
print("Data not formatted properly...")
def __init__(
self,
NUM_SENSORS=2,
BAUDRATE=19200,
PORT=None,
READ_RATE=10,
):
if PORT is None:
PORT = check_output(
['rosrun', 'gudrun', 'get_usb_device_by_ID',
'encoder_mega']).strip(),
rospy.init_node('listen_to_ultrasound')
self.NUM_SENSORS = NUM_SENSORS
self.BAUDRATE = BAUDRATE
self.PORT = PORT
self.READ_RATE = READ_RATE
self.ser = serial.Serial(self.PORT, self.BAUDRATE)
self.publishers = [
rospy.Publisher('sensors/ultrasound_%d' % i, Range, queue_size=1)
for i in range(self.NUM_SENSORS)
]
m = Range()
m.radiation_type = Range.ULTRASOUND
m.field_of_view = 22.5 * 3.14159 / 180.
m.min_range = 6.
m.max_range = 200.
m.range = 0
m.header = Header()
self.message = m
self.loop()
def update_physics(self, delay, quadcopterId, quadcopter_controller,
cmd_sub, imu_pub, range_pub):
vol_meas = p.getBaseVelocity(quadcopterId)
one_over_sample_rate = 1 / delay
previous_publish = time.perf_counter()
try:
while not rospy.is_shutdown():
start = time.perf_counter()
# the controllers are evaluated at a slower rate, only once in the
# control_subsample times the controller is evaluated
pos_meas, quaternion_meas = p.getBasePositionAndOrientation(
quadcopterId)
previous_vol_meas = np.array(vol_meas)
vol_meas = np.array(p.getBaseVelocity(quadcopterId))
#print(vol_meas, type(vol_meas))
acc = (vol_meas - previous_vol_meas) * one_over_sample_rate
#print("VOL MEAS", previous_vol_meas, acc)
if quadcopter_controller.sample == 0:
quadcopter_controller.sample = control_subsample
#force_act1,force_act2,force_act3,force_act4,moment_yaw = quadcopter_controller.update_control(pos_meas,quaternion_meas)
actual_values = cmd_sub.get_control()
vectorizer = np.zeros([5, 3])
vectorizer[:, 2] = actual_values
#print("input forces ", actual_values)
force_act1, force_act2, force_act3, force_act4, moment_yaw = vectorizer
#print("Well well well\t\t", vectorizer, force_act1, moment_yaw)
else:
quadcopter_controller.sample -= 1
# apply forces/moments from controls etc:
# (do this each time, because forces and moments are reset to zero after a stepSimulation())
#assuming right is -y
p.applyExternalForce(quadcopterId, -1, force_act1,
[arm_length, -arm_length, 0.],
p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act2,
[-arm_length, -arm_length, 0.],
p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act3,
[-arm_length, arm_length, 0.],
p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act4,
[arm_length, arm_length, 0.],
p.LINK_FRAME)
# for the yaw-control:
p.applyExternalTorque(quadcopterId, -1, moment_yaw,
p.LINK_FRAME)
# do simulation with pybullet:
p.stepSimulation()
header = Header()
header.frame_id = 'Body'
header.stamp = rospy.Time.now()
Tsample_physics
imu_message = Imu()
imu_message.header = header
imu_message.orientation.x = quaternion_meas[0]
imu_message.orientation.y = quaternion_meas[1]
imu_message.orientation.z = quaternion_meas[2]
imu_message.orientation.w = quaternion_meas[3]
imu_message.linear_acceleration.x = acc[0, 0]
imu_message.linear_acceleration.y = acc[0, 1]
imu_message.linear_acceleration.z = acc[0, 2]
msg = Range()
msg.header = header
msg.max_range = 200
msg.min_range = 0
msg.range = pos_meas[2]
if start - previous_publish > 1 / 100:
imu_pub.publish(imu_message)
range_pub.publish(msg)
previous_publish = start
#print("BUBLISHED RANON /simulation/infrared_sensor_node", msg.range)
# delay than repeat
calc_time = time.perf_counter() - start
if (calc_time > delay):
#print("Time to update physics is {} and is more than 20% of the desired update time ({}).".format(calc_time,delay))
pass
else:
# print("calc_time = ",calc_time)
while (time.perf_counter() - start < delay):
time.sleep(delay / 10)
except KeyboardInterrupt:
self.ctrl_c_reset()
top_topic = "top_camera_range"
front_topic = "front_camera_range"
s_range = 4
fov = pi/4
top_pub = rospy.Publisher(top_topic, Range, queue_size = 10)
front_pub = rospy.Publisher(front_topic, Range, queue_size = 10)
rospy.init_node('range_publisher')
r = rospy.Rate(10)
while not rospy.is_shutdown():
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = top_frame
rg_top = Range()
rg_top.header = h
rg_top.field_of_view = fov
rg_top.max_range = s_range
rg_top.min_range = s_range
rg_top.range = s_range
rg_top.radiation_type = Range.INFRARED
rg_front = copy.deepcopy(rg_top)
rg_front.header.frame_id = front_frame
top_pub.publish(rg_top)
front_pub.publish(rg_front)
r.sleep()
from sensor_msgs.msg import Range
from std_msgs.msg import Header
rospy.init_node('lidar_range')
pub = [None, None, None, None]
# 4개의 토픽 발행
for i in range(4):
name = 'scan' + str(i)
pub[i] = rospy.Publisher(name, Range, queue_size=1)
msg = Range()
h = Header()
h.frame_id = "sensorXY"
msg.header = h
msg.radiation_type = Range().ULTRASOUND
msg.min_range = 0.02 # 2cm
msg.max_range = 2.0 # 2m
msg.field_of_view = (30.0 / 180.0) * 3.14
while not rospy.is_shutdown():
msg.header.stamp = rospy.Time.now()
# msg.range에 장애물까지의 거리를 미터 단위로 넣고 토픽을 발행한다.
msg.range = 0.4
for i in range(4):
pub[i].publish(msg)
msg.range += 0.4
