Commit 39763088 authored by hameck's avatar hameck
Browse files

add maxAngle and figure in emulator

parent 671fbc93
...@@ -17,5 +17,6 @@ ...@@ -17,5 +17,6 @@
<param name="enableImageTopic" value="True"/> <param name="enableImageTopic" value="True"/>
<param name="enableScanTopic" value="True"/> <param name="enableScanTopic" value="True"/>
<param name="enableDataTopic" value="True"/> <param name="enableDataTopic" value="True"/>
<param name="maxAngle" value="400"/>
</node> </node>
</launch> </launch>
\ No newline at end of file
...@@ -4,6 +4,8 @@ import time ...@@ -4,6 +4,8 @@ import time
import errno import errno
import math import math
import numpy as np import numpy as np
import random
verbose = False verbose = False
payload_dict = definitions.payload_dict_all payload_dict = definitions.payload_dict_all
...@@ -182,7 +184,7 @@ class Serial: ...@@ -182,7 +184,7 @@ class Serial:
def generateRandomData(self): def generateRandomData(self):
sigma = 10 sigma = 10
mu = 100 mu = 100 + int(30.0 * math.sin((self._angle + random.randrange(40)) / 40.))
self._data = "".join([chr(int(255 * np.exp(-np.power(x - mu, 2.) / (2 * np.power(sigma, 2.))))) self._data = "".join([chr(int(255 * np.exp(-np.power(x - mu, 2.) / (2 * np.power(sigma, 2.)))))
for x in range(self._number_of_samples)]) for x in range(self._number_of_samples)])
......
...@@ -35,6 +35,7 @@ firstRequest = True ...@@ -35,6 +35,7 @@ firstRequest = True
enableImageTopic = False enableImageTopic = False
enableScanTopic = False enableScanTopic = False
enableDataTopic = False enableDataTopic = False
maxAngle = None
def callback(config, level): def callback(config, level):
...@@ -87,6 +88,7 @@ def main(): ...@@ -87,6 +88,7 @@ def main():
enableImageTopic = rospy.get_param('~enableImageTopic', True) enableImageTopic = rospy.get_param('~enableImageTopic', True)
enableScanTopic = rospy.get_param('~enableScanTopic', True) enableScanTopic = rospy.get_param('~enableScanTopic', True)
enableDataTopic = rospy.get_param('~enableDataTopic', True) enableDataTopic = rospy.get_param('~enableDataTopic', True)
maxAngle = rospy.get_param('~maxAngle', 400)
# Output and ROS parameters # Output and ROS parameters
step = int(rospy.get_param('~step', 1)) step = int(rospy.get_param('~step', 1))
...@@ -136,8 +138,8 @@ def main(): ...@@ -136,8 +138,8 @@ def main():
transmitDuration, samplePeriod, numberOfSamples) transmitDuration, samplePeriod, numberOfSamples)
angle_increment = 2 * pi * step / 400 angle_increment = 2 * pi * step / 400
ranges = [0] * (400 // step) ranges = [0] * (maxAngle // step)
intensities = [0] * (400 // step) intensities = [0] * (maxAngle // step)
# Get sonar response # Get sonar response
data = getSonarData(sensor, angle) data = getSonarData(sensor, angle)
...@@ -193,7 +195,7 @@ def main(): ...@@ -193,7 +195,7 @@ def main():
publishImage(image, imagePub, bridge) publishImage(image, imagePub, bridge)
angle = (angle + step) % 400 # TODO: allow users to set a scan FOV angle = (angle + step) % maxAngle
rate.sleep() rate.sleep()
......
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