Sonar Board Problem!

1

Hi

I would like to read one of the sonars included in Magni Robot. I saw that not all the time I receive the correct data from the sonar. Here is my code:

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Pose
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Range
import math
import RPi.GPIO as GPIO
import time
from std_srvs.srv import Empty

def move(velocity_publisher, sensor_publisher, speed, distance, is_forward):
velocity_message = Twist()
sensor_message = Range()
global x
global y
global sens
x0=x
y0=y
if (is_forward)
velocity_message.linear.x = abs(speed)
else:
velocity_message.linear.x = -abs(speed)
distance_moved = 0.0
loop_rate = rospy.Rate(10)
while True:
rospy.loginfo(“Robot moves forwards”)
velocity_publisher.publish(velocity_message)
rospy.loginfo('Sensor 3: ')
sensor_publisher.publish(sensor_message)
loop_rate.sleep()
distance_moved = abs(math.sqrt(((x-x0) ** 2) + ((y-y0) ** 2)))
print(‘Print Distance Moved’,distance_moved)
print(‘X coordinate’,x)
print(‘Sensor 3 range’, sens)
if (distance_moved > distance):
rospy.loginfo(“reached”)
break
velocity_message.linear.x = 0
velocity_publisher.publish(velocity_message)

def poseCallback(pose_message):
global x
global y, yaw
x = pose_message.pose.pose.position.x
y = pose_message.pose.pose.position.y
yaw = pose_message.pose.pose.orientation.w

if name == ‘main’:
try:
rospy.init_node(‘turtlesim_motion_pose’, anonymous=True)
cmd_vel_topic = ‘/cmd_vel’
velocity_publisher = rospy.Publisher(cmd_vel_topic, Twist, queu$
position_topic = ‘/odom’
pose_subscriber = rospy.Subscriber(position_topic, Odometry, po$
sensor3_topic = ‘/pi_sonar/sonar_3’
sensor_publisher = rospy.Publisher(sensor3_topic, Range, queue_$
sensor_range3 = rospy.Subscriber(sensor3_topic, Range, sensorCa$
time.sleep(2)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(11, GPIO.OUT)
GPIO.output(11, GPIO.LOW)
move(velocity_publisher, sensor_publisher, 0.2, 1, True)
GPIO.output(11, GPIO.HIGH)
time.sleep(5)
GPIO.output(11, GPIO.LOW)
move(velocity_publisher, sensor_publisher, 0.2, 1, False)
GPIO.output(11, GPIO.HIGH)
time.sleep(5)
GPIO.output(11, GPIO.LOW)
except rospy.ROSInterruptException:
rospy.loginfo(‘node_terminated.’)

Blockquote

Here is the result:

Screenshot from 2021-05-31 16-31-57
Screenshot from 2021-05-31 16-31-572560×1440 453 KB

2

If you would like to see the data from any or all of the sonar sensors then there are two steps for the Magni product.

  1. Enable the sonar ROS node. See 1/3 way down this page: Sonar Board Installation | Learn Ubiquity Robots and ROS

  2. Have a node that ‘subscribes’ to the ROS topic called /sonars and read the Range messages. We have a couple examples of this. Perhaps see this demo demos/sonar_wanderer at master · UbiquityRobotics/demos · GitHub
    If you do a ‘git pull’ from within ~/catkin_ws/src/demos then you would get the sonar_wanderer demo. That would show you how to read our ROS topics.

To make a sonar work by itself without the Magni software is more of an experiment. Perhaps that is what you are trying to do? If that is your goal perhaps seek out some form of DIY demo experiment somebody may have done on the web somewhere. If you are trying to do that while Magni is running you would have to disable our control of the sonars and this would be the default for Magni as if you had not enabled them using step 1 above.

Reading sonars outside of our code directly from the Raspberry Pi you need to search for some demo that runs on the Raspberry Pi Python 2.7 and controls what is called the ‘HC-SR04’ sonar. I am very sure there are examples somewhere on the web but we don’t do that, we read sonars in our sonar node

3

Also it is important for your test to see the same readings of a sonar that you start simple. Have the sonar you are investigating pointing straight into a wall that is 1 meter away or some fixed distance. When sonars are pointing at an angled wall of 45 degrees the sonar return echo is not always seen well so that leads to odd readings. Also if a sonar has some wall 3 meters away and you place some small pole in front of the sonar that depending on the type of material or angle of the pole (if squiare) the sonar may sometimes tell you where the pole is but may also if it gets weak reflection from the pole tell you the distance to the wall behind it. Worse case is if a sonar points into a very busy area like a bunch of legs from some chairs it can get very ‘confused’. Hope this helps you understand what sort of issues are experienced with sonar units.