Cris ThomasJiss Joseph Thomas
Published © GPL3+

Collision Evasion System for Autonomous Robots

A robust, inexpensive, yet powerful collision detection system for the purpose of small autonomous robots

ExpertWork in progress20 hours499
Collision Evasion System for Autonomous Robots

Things used in this project

Hardware components

Raspberry Pi 3 Model B
Raspberry Pi 3 Model B
×1
Stereo Camera
Custom made from Microsoft cameras of the same make
×1
Neural Compute Stick 2
Intel Neural Compute Stick 2
×1

Software apps and online services

OpenVINO toolkit
Intel OpenVINO toolkit
OpenCV
OpenCV

Story

Read more

Schematics

Configuration

Code

Evasion_implementation

Python
Evasive algorithm developed for micro UAVs
#!/usr/bin/env python
import rospy
import thread
import time
import mavros
from geometry_msgs.msg import PoseStamped, Quaternion
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, SetMode
from std_msgs.msg import String
from mavros_msgs.srv import *
from math import *
from tf.transformations import quaternion_from_euler
mavros.set_namespace()
# Set collision threshold
double collision_thresh = 2.0 #meters
local_pos_pub = rospy.Publisher(mavros.get_topic('setpoint_position', 'local'), PoseStamped, queue_size=10)
target_pose = PoseStamped()
target_pose.pose.position.x = 0.0
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = 0.0
thread_stop_flag = True
posthread = 0
def position_tracking_thread(target_pose):
global thread_stop_flag
rate = rospy.Rate(10)
while not thread_stop_flag:
local_pos_pub.publish(target_pose)
rate.sleep()
def position_control(x=0, y=0, z=0):
print "begining position control"
global thread_stop_flag
global target_pose
global posthread
thread_stop_flag = True
target_pose.pose.position.x = float(x)
target_pose.pose.position.y = float(y)
target_pose.pose.position.z = float(z)
yaw_degrees = 0  # North
yaw = radians(yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
target_pose.pose.orientation = Quaternion(*quaternion)
target_pose.header.frame_id = "base_footprint"
now = rospy.get_rostime()
# Update timestamp and publish pose
target_pose.header.stamp = rospy.Time.now()
try:
print "waiting for previous thread to join"
#	posthread.join()
time.sleep(1)
print "thread joined, starting new thread"
thread_stop_flag = False
posthread = thread.start_new_thread( position_tracking_thread, (target_pose,) )
except e:
print "Error in threeady thingy ",e
print "waiting to start offboard mode"
time.sleep(1)
setOffboardMode()
print "offboard mode commanded"
def setOffboardMode():
rospy.wait_for_service('/mavros/set_mode')
try:
flightModeService = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode)
isModeChanged = flightModeService(custom_mode='OFFBOARD')
except rospy.ServiceException, e:
print "service set_mode call failed: %s. OFFBOARD Mode could not be set"%e
def setStabilizedMode():
global thread_stop_flag
thread_stop_flag = True
rospy.wait_for_service('/mavros/set_mode')
try:
flightModeService = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode)
isModeChanged = flightModeService(custom_mode='STABILIZED')
except rospy.ServiceException, e:
print "service set_mode call failed: %s. STABILIZED Mode could not be set. Check that GPS is enabled"%e
def setTakeoffMode():
rospy.wait_for_service('/mavros/cmd/takeoff')
try:
takeoffService = rospy.ServiceProxy('/mavros/cmd/takeoff', mavros_msgs.srv.CommandTOL)
takeoffService(altitude = 2, latitude = 0, longitude = 0, min_pitch = 0, yaw = 0)
except rospy.ServiceException, e:
print "Service takeoff call failed: %s"%e
def setLandMode():
rospy.wait_for_service('/mavros/cmd/land')
try:
landService = rospy.ServiceProxy('/mavros/cmd/land', mavros_msgs.srv.CommandTOL)
isLanding = landService(altitude = 0, latitude = 0, longitude = 0, min_pitch = 0, yaw = 0)
except rospy.ServiceException, e:
print "service land call failed: %s. The vehicle cannot land "%e
def setArm():
rospy.wait_for_service('/mavros/cmd/arming')
try:
armService = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool)
armService(True)
except rospy.ServiceException, e:
print "Service arm call failed: %s"%e
def setDisarm():
rospy.wait_for_service('/mavros/cmd/arming')
try:
armService = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool)
armService(False)
except rospy.ServiceException, e:
print "Service disarm call failed: %s"%e
def pingCallback(double &msg):
if(msg.data < collision_thresh):
setOffboardMode()
position_control(x=0, y=0.5, z=0)
def menu():
print "Press"
print "1: to set mode to OFFBOARD"
print "2: to set mode to STABILIZE"
print "3: to set mode to ARM the drone"
print "4: to set mode to DISARM the drone"
print "5: to set mode to TAKEOFF"
print "6: to set mode to LAND"
print "7: Position Control"
def myLoop():
x='1'
while ((not rospy.is_shutdown())):
menu()
x = raw_input("Enter your input: ");
if (x=='1'):
setOffboardMode()
elif(x=='2'):
setStabilizedMode()
elif(x=='3'):
setArm()
elif(x=='4'):
setDisarm()
elif(x=='5'):
setTakeoffMode()
elif(x=='6'):
setLandMode()
elif(x=='7'):
xpos = raw_input("Enter x position ")
ypos = raw_input("Enter y position ")
zpos = raw_input("Enter z position ")
position_control(xpos,ypos,zpos)
else:
print "Exit"
break
if __name__ == '__main__':
rospy.init_node('dronemap_node', anonymous=True)
rospy.Subscriber("/ping_dist/data", double, pingCallback)
myLoop()

Credits

Cris Thomas

Cris Thomas

13 projects • 49 followers
Electronics and Aerospace engineer with a dedicated history in Research and Development
Jiss Joseph Thomas

Jiss Joseph Thomas

10 projects • 39 followers
Co-Researcher Trainee (Artificial Intelligence) at Creopedia Business Intelligence Pvt.Ltd, Kochi,

Comments