Unverified Commit 204fac52 authored by Anas Mazouni's avatar Anas Mazouni Committed by GitHub
Browse files

Merge pull request #15 from CentraleNantesRobotics/develop

Feature: Ping360 for SLAM applications
parents ee99c3a8 daeb598d
......@@ -24,6 +24,15 @@
"code",
"test"
]
},
{
"login": "tomlogan501",
"name": "tomlogan501",
"avatar_url": "https://avatars3.githubusercontent.com/u/56969577?v=4",
"profile": "https://github.com/tomlogan501",
"contributions": [
"ideas"
]
}
],
"contributorsPerLine": 7,
......
......@@ -91,6 +91,10 @@ While continuously rotating the sonar, it publishes two types of messages:
uint8 range # range value [m]
uint8[] intensities # intensity data [0-255]. This is the actual data received from the sonar
* **`/ping360_node/sonar/scan`** ([sensor_msgs/LaserScan])
Publishes a LaserScan msg with ranges detected with a certain intensity threshold.
## Bugs & Feature Requests
......@@ -113,6 +117,7 @@ Please report bugs and request features using the [Issue Tracker](https://github
<tr>
<td align="center"><a href="https://github.com/Hameck"><img src="https://avatars2.githubusercontent.com/u/14954732?v=4" width="100px;" alt=""/><br /><sub><b>Henrique Martinez Rocamora</b></sub></a><br /><a href="https://github.com/CentraleNantesRobotics/ping360_sonar_python/commits?author=Hameck" title="Code">💻</a> <a href="https://github.com/CentraleNantesRobotics/ping360_sonar_python/commits?author=Hameck" title="Tests">⚠️</a></td>
<td align="center"><a href="https://stormix.co"><img src="https://avatars2.githubusercontent.com/u/18377687?v=4" width="100px;" alt=""/><br /><sub><b>Anas Mazouni</b></sub></a><br /><a href="https://github.com/CentraleNantesRobotics/ping360_sonar_python/commits?author=Stormiix" title="Code">💻</a> <a href="https://github.com/CentraleNantesRobotics/ping360_sonar_python/commits?author=Stormiix" title="Tests">⚠️</a></td>
<td align="center"><a href="https://github.com/tomlogan501"><img src="https://avatars3.githubusercontent.com/u/56969577?v=4" width="100px;" alt=""/><br /><sub><b>tomlogan501</b></sub></a><br /><a href="#ideas-tomlogan501" title="Ideas, Planning, & Feedback">🤔</a></td>
</tr>
</table>
......
......@@ -13,5 +13,6 @@ gen.add("transmitFrequency", int_t, 0, "Transmit Frequency (kHz)", 740, 50
gen.add("speedOfSound", int_t, 0, "Speed of sound (m/s)", 1450, 1450, 1550)
gen.add("step", int_t, 0, "Filling Step", 1, 1,10)
gen.add("queueSize", int_t, 0, "Queue Size", 1, 0,10)
gen.add("threshold", int_t, 0, "Intensity threshold", 200, 0, 255)
exit(gen.generate(PACKAGE, "ping360_sonar", "sonar"))
\ No newline at end of file
......@@ -12,5 +12,6 @@
<param name="sonarRange" value="1"/>
<param name="speedOfSound" value="1500"/>
<param name="queueSize" value="1"/>
<param name="threshold" value="100"/>
</node>
</launch>
\ No newline at end of file
......@@ -11,7 +11,7 @@ import rospy
from cv_bridge import CvBridge, CvBridgeError
from dynamic_reconfigure.server import Server
from sensor_msgs.msg import Image
from std_msgs.msg import String
from sensor_msgs.msg import LaserScan
from ping360_sonar.cfg import sonarConfig
from ping360_sonar.msg import SonarEcho
......@@ -19,7 +19,7 @@ from sensor import Ping360
def callback(config, level):
global updated, gain, numberOfSamples, transmitFrequency, transmitDuration, sonarRange, speedOfSound, samplePeriod, debug, step, imgSize, queue_size, p
global updated, gain, numberOfSamples, transmitFrequency, transmitDuration, sonarRange, speedOfSound, samplePeriod, debug, step, imgSize, queue_size, p, threshold
# Update Ping 360 Parameters
gain = config['gain']
numberOfSamples = config['numberOfSamples']
......@@ -31,6 +31,7 @@ def callback(config, level):
debug = config['debug']
step = config['step']
queue_size = config['queueSize']
threshold = config['threshold']
rospy.loginfo("Reconfigure Request")
updated = True
return config
......@@ -46,6 +47,7 @@ def main():
# Topic publishers
imagePub = rospy.Publisher("/ping360_node/sonar/images", Image, queue_size=queue_size)
rawPub = rospy.Publisher("/ping360_node/sonar/data", SonarEcho, queue_size=queue_size)
laserPub = rospy.Publisher("/ping360_node/sonar/scan", LaserScan, queue_size=queue_size)
# Initialize and configure the sonar
updateSonarConfig(gain, transmitFrequency, transmitDuration, samplePeriod, numberOfSamples)
......@@ -53,10 +55,15 @@ def main():
# Create a new mono-channel image
image = np.zeros((imgSize, imgSize, 1), np.uint8)
# Initial the LaserScan Intensities & Ranges
angle_increment = 2*pi*step / 400
ranges = [0]*(400 // step)
intensities = [0]*(400 // step)
# Center point coordinates
center = (float(imgSize/2),float(imgSize/2))
rate = rospy.Rate(100) # 10hz
rate = rospy.Rate(100) # 100hz
while not rospy.is_shutdown():
......@@ -71,6 +78,20 @@ def main():
rawDataMsg = generateRawMsg(angle, data) # TODO: check for empty responses
rawPub.publish(rawDataMsg)
# Contruct and publish Sonar scan msg
scanDataMsg = generateScanMsg(ranges, intensities) # TODO: check for empty responses
index = int(round((angle * 2 * pi / 400) / angle_increment))
detectedRange = next((x for x in data if x >= threshold), None) # Get the first high intensity value
if detectedRange:
detectedIndex = data.index(detectedRange) # Get the intensity index
distance = calculateRange((1 + detectedIndex), samplePeriod, speedOfSound) # The index+1 represents the number of samples which then can be used to deduce the range
# TODO: if distance >= 0.75 and distance <= sonarRange:
ranges[index] = min(distance, ranges[index]) * 100 # In some instance where multiple measures are done in a single angle_increment, we take the closest one
intensities[index] = detectedRange if distance <= ranges[index] else intensities[index]
if debug:
print("Detected object at {} grad : {}m - intensity {}%".format(angle, ranges[index], float(intensities[index]*100/255)))
laserPub.publish(scanDataMsg)
# Contruct and publish Sonar image msg
linear_factor = float(len(data)) / float(center[0]) #TODO: this should probably be range/pixelsize
try:
......@@ -128,6 +149,27 @@ def generateRawMsg(angle, data):
msg.intensities = data
return msg
def generateScanMsg(ranges, intensities):
"""
Generates the laserScan message for the scan topic
Params:
angle int (Gradian Angle)
data list List of intensities
"""
msg = LaserScan()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'sonar_frame'
msg.angle_min = 0
msg.angle_max = 2 * pi
msg.angle_increment = 2*pi*step / 400
msg.time_increment = 0
msg.range_min = .75
msg.range_max = sonarRange
msg.ranges = ranges
msg.intensities = intensities
return msg
def publishImage(image, imagePub, bridge):
try:
imagePub.publish(bridge.cv2_to_imgmsg(image, "mono8"))
......@@ -135,6 +177,13 @@ def publishImage(image, imagePub, bridge):
rospy.logwarn("Failed to publish sensor image")
print(e)
def calculateRange(numberOfSamples, samplePeriod, speedOfSound, _samplePeriodTickDuration = 25e-9):
# type: (float, int, float, float) -> float
"""
Calculate the range based in the duration
"""
return numberOfSamples * speedOfSound * _samplePeriodTickDuration * samplePeriod / 2
def calculateSamplePeriod(distance, numberOfSamples, speedOfSound, _samplePeriodTickDuration = 25e-9):
# type: (float, int, int, float) -> float
"""
......@@ -198,6 +247,8 @@ samplePeriod = calculateSamplePeriod(sonarRange, numberOfSamples, speedOfSound)
transmitDuration = adjustTransmitDuration(sonarRange, samplePeriod, speedOfSound)
debug = rospy.get_param('~debug', True)
threshold = rospy.get_param('~threshold', 200) # 0-255
# Output and ROS parameters
step = rospy.get_param('~step', 1)
imgSize = rospy.get_param('~imgSize', 500)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment