Commit 48f967c1 authored by Anne Donnet's avatar Anne Donnet
Browse files

Final code for competiton

parent dfd6bfdd
......@@ -52,16 +52,16 @@ find_package(catkin REQUIRED COMPONENTS
FILES
MotorSpeed.msg
BottlePosition.msg
Localisation.msg
# Message1.msg
# Message2.msg
)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
add_service_files(
FILES
GetLocalisation.srv
)
## Generate actions in the 'action' folder
# add_action_files(
......@@ -165,6 +165,7 @@ include_directories(
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS
scripts/bottle_position.py
scripts/get_localisation.py
scripts/main.py
scripts/uart.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
......
<launch>
<!-- MAIN -->
<node pkg="fri_robot" type="main.py" name="main" output="screen">
</node>
<!-- STM32 COMMUNICATION -->
<node pkg="fri_robot" type="uart.py" name="uart" output="screen">
</node>
<!-- BOTTLE POSITION COMPUTER -->
<node pkg="fri_robot" type="bottle_position.py" name="bottle_position" output="screen">
</node>
<!-- LOCALISATION -->
<node pkg="fri_robot" type="get_localisation.py" name="get_localisation" output="screen">
</node>
<!-- DETECTNET -->
<arg name="model_name" default="coco-bottle"/>
<arg name="model_path" default=""/>
<arg name="prototxt_path" default=""/>
<arg name="class_labels_path" default=""/>
<arg name="input_blob" default=""/>
<arg name="output_cvg" default=""/>
<arg name="output_bbox" default=""/>
<arg name="overlay_flags" default="box,labels,conf"/>
<arg name="mean_pixel_value" default="0.0"/>
<arg name="threshold" default="0.5"/>
<node pkg="ros_deep_learning" type="detectnet" name="detectnet" output="screen">
<remap from="/detectnet/image_in" to="/video_source/raw"/>
<param name="model_name" value="$(arg model_name)"/>
<param name="model_path" value="$(arg model_path)"/>
<param name="prototxt_path" value="$(arg prototxt_path)"/>
<param name="class_labels_path" value="$(arg class_labels_path)"/>
<param name="input_blob" value="$(arg input_blob)"/>
<param name="output_cvg" value="$(arg output_cvg)"/>
<param name="output_bbox" value="$(arg output_bbox)"/>
<param name="overlay_flags" value="$(arg overlay_flags)"/>
<param name="mean_pixel_value" value="$(arg mean_pixel_value)"/>
<param name="threshold" value="$(arg threshold)"/>
</node>
<!-- VIDEO SOURCE -->
<arg name="input" default="csi://0"/>
<arg name="input_width" default="0"/>
<arg name="input_height" default="0"/>
<arg name="input_codec" default="unknown"/>
<arg name="input_loop" default="0"/>
<include file="$(find ros_deep_learning)/launch/video_source.ros1.launch">
<arg name="input" value="$(arg input)"/>
<arg name="input_width" value="$(arg input_width)"/>
<arg name="input_height" value="$(arg input_height)"/>
<arg name="input_codec" value="$(arg input_codec)"/>
<arg name="input_loop" value="$(arg input_loop)"/>
</include>
</launch>
float64 pos_x
float64 pos_y
float64 angle
float64 left_speed
float64 right_speed
float64 duration
#!/usr/bin/env python
import rospy
import math
from std_msgs.msg import Bool
from vision_msgs.msg import Detection2DArray
from std_msgs.msg import Float64MultiArray
from fri_robot.msg import BottlePosition as bottlePosition
def compute_distance(size_x, size_y, FL):
min_size = min(size_x, size_y)
BOTTLE_DIAMETER = 63
distance = FL * BOTTLE_DIAMETER / min_size
return distance
def compute_fl(size_x, size_y):
min_size = min(size_x, size_y)
BOTTLE_DIAMETER = 63
distance = 297
fl = distance * min_size / BOTTLE_DIAMETER
return fl
def compute_angle(distance, center_x, FL, IW):
w = distance * IW / FL
dx = (center_x - (IW/2)) / IW * w
return -math.asin(dx/distance)
def callback(data):
verbose = False
MIN_SCORE = 0.6
MAX_DIST = 2000
detections = data.detections
nb_det = len(detections)
bottle_pos = bottlePosition()
if(verbose):
rospy.loginfo("detected %s bottles", nb_det)
for det in detections:
# rospy.loginfo("new det")
score = det.results[0].score
bbox = det.bbox
center = bbox.center
size_x = bbox.size_x
size_y = bbox.size_y
if(score >= MIN_SCORE):
if(verbose):
rospy.loginfo("============================")
rospy.loginfo("score: %s", score)
rospy.loginfo("center: %s", center)
rospy.loginfo("size: %s x %s", size_x, size_y)
FOCAL_LENGTH = 1700
IMAGE_WIDTH = 1280
distance = compute_distance(size_x, size_y, FOCAL_LENGTH)
angle = compute_angle(distance, center.x, FOCAL_LENGTH, IMAGE_WIDTH)
# fl = compute_fl(size_x, size_y)
# rospy.loginfo("estimated fl: %s", fl)
# rospy.loginfo("computed distance: %s", distance)
# rospy.loginfo("computed angle: %s", angle)
bottle_pos.distance = distance
bottle_pos.angle = angle
if(verbose):
rospy.loginfo("=<<<<<<<<<<<<<<<<<<<<<<<<<<=")
if(distance < MAX_DIST):
position_pub.publish(bottle_pos)
if __name__ == '__main__':
position_pub = rospy.Publisher('bottle_position', bottlePosition, queue_size=10)
ask = True
detection_asker = rospy.Publisher('frame_asking', Bool, queue_size=10)
rospy.init_node('bottle_position', anonymous=False)
rospy.Subscriber("/detectnet/detections", Detection2DArray, callback)
rate = rospy.Rate(0.5)
while not rospy.is_shutdown():
detection_asker.publish(ask)
rate.sleep()
# spin() simply keeps python from exiting until this node is stopped
#!/usr/bin/env python3
import rospy
from std_msgs.msg import Bool
from fri_robot.msg import Localisation as localisation
import cv2 as cv
import numpy as np
import math
import random
import time
import localisation_helper as loc_helper
from fri_robot.srv import GetLocalisation, GetLocalisationResponse
def define_video_object():
WHITE_BALANCE = str(4)
MIN_EXP_TIME = str(5000)
MAX_EXP_TIME = str(5000)
camSet='nvarguscamerasrc ee-mode=2 ' \
'sensor_id=1 aelock=True wbmode='+WHITE_BALANCE+ ' exposuretimerange="'+MIN_EXP_TIME+' '+MAX_EXP_TIME+'"! ' \
'video/x-raw(memory:NVMM), width=1920, height=1080, format=NV12, framerate=30/1 ! ' \
'nvvidconv flip-method=0 ! ' \
'video/x-raw, width=1920, height=1080, format=BGRx ! ' \
'videoconvert ! video/x-raw, format=BGR ! appsink'
return cv.VideoCapture(camSet,cv.CAP_GSTREAMER)
def cam_loca_to_center(x, y, angle):
if(x is None or y is None or angle is None):
return x, y, angle
else:
cpx = cpy = 0
CENTER_TO_CAM_X = 0.28
CENTER_TO_CAM_Y = 0
cpx = x + math.cos(angle) * CENTER_TO_CAM_X
cpy = y + math.sin(angle) * CENTER_TO_CAM_X
return cpx, cpy, angle
def loca_from_video_object(video_object, i):
# get the last grabbed frame
captured = video_object.retrieve(img)
if captured:
if(debug):
tag = random.randint(0, 10000000)
cv.imwrite("/home/frirobot/ros_catkin_ws/src/fri_robot/scripts/img_wb_"+str(tag)+"_moving.jpg", img)
rospy.loginfo("CORRESPONDING TAG: "+str(tag))
# Compute localisation
loc = GetLocalisationResponse()
cam_x, cam_y, cam_angle = loc_helper.img_to_pos(img)
#if(debug):
# rospy.loginfo("CURRENT CAM LOCA: X: "+str(cam_x)+" Y: "+str(cam_y)+ "A: "+str(cam_angle))
# Adjust localisation between the wheels instead of on camera
loc.pos_x, loc.pos_y, loc.angle = cam_loca_to_center(cam_x, cam_y, cam_angle)
# Return new localisation
if (not loc.pos_x):
loc.pos_x = loc.pos_y = loc.angle = -1
return loc
def get_localisation(req):
return loca_from_video_object(video_object, req)
if __name__ == "__main__":
NB_PICS_TO_SET_UP = 50
debug = False
video_object = define_video_object()
capture, img = video_object.read()
for i in range(0, NB_PICS_TO_SET_UP):
video_object.grab()
rospy.init_node('localisation_service', anonymous=False)
localisation_service = rospy.Service('compute_localisation', GetLocalisation, get_localisation)
while not rospy.is_shutdown():
video_object.grab()
rospy.spin()
This diff is collapsed.
#!/usr/bin/env python3
import rospy
import math
import time
from std_msgs.msg import Bool
from std_msgs.msg import Float64
from fri_robot.msg import MotorSpeed as motorSpeed
from fri_robot.msg import BottlePosition as bottlePosition
from fri_robot.srv import GetLocalisation, GetLocalisationResponse
def us_callback(data):
if (debug):
rospy.loginfo("received us_message: %s", data)
def bottle_detected(data):
FORWARD_DIST = 0.6
dist = data.distance
angle = data.angle
if (verbose):
rospy.loginfo("detected dist "+str(round(dist, 2))+ " and angle "+str(round(angle, 2)) )
move(angle, FORWARD_DIST)
def speed_to_pwm(speed, MIN_SPEED, MAX_SPEED):
if speed < MIN_SPEED:
return 10
elif speed > MAX_SPEED:
return 90
else:
return 10 + speed / MAX_SPEED * 80
def manual_stop():
if (debug):
rospy.loginfo("========================> MANUAL STOP")
motor_speed = motorSpeed()
motor_speed.left_speed = 10
motor_speed.right_speed = 10
motor_speed.duration = 0.001
if motor_speed.duration > 0:
motor_pub.publish(motor_speed)
time.sleep(motor_speed.duration)
def turn(angle):
if (abs(angle) > 0.1):
# Checks if the opposite angle requires less turning
opposite_angle = angle - math.pi*2*sign(angle)
if(abs(opposite_angle) < abs(angle)):
angle = opposite_angle
AXLE_LENGTH = 0.22
WHEEL_RADIUS = 0.04
MIN_SPEED_MOTOR = 35
MAX_SPEED_MOTOR = 10000/18
MIN_SPEED = MIN_SPEED_MOTOR * 2 * math.pi / 60
MAX_SPEED = MAX_SPEED_MOTOR * 2 * math.pi / 60
FORWARD_SPEED = MIN_SPEED
turn_time = abs(angle) * AXLE_LENGTH / MIN_SPEED / WHEEL_RADIUS / 2
turn_time = turn_time * 0.93
motor_left = 0
motor_right = 0
if angle > 0:
motor_left = -speed_to_pwm(MIN_SPEED, MIN_SPEED, MAX_SPEED)
motor_right = speed_to_pwm(MIN_SPEED, MIN_SPEED, MAX_SPEED)
elif angle < 0:
motor_left = speed_to_pwm(MIN_SPEED, MIN_SPEED, MAX_SPEED)
motor_right = -speed_to_pwm(MIN_SPEED, MIN_SPEED, MAX_SPEED)
elif angle == 0:
motor_left = motor_right = 0
motor_speed = motorSpeed()
motor_speed.left_speed = motor_left
motor_speed.right_speed = motor_right
motor_speed.duration = turn_time
if (debug):
rospy.loginfo("========================> TURN : "+ str(round(turn_time, 2)))
if turn_time > 0:
motor_pub.publish(motor_speed)
time.sleep(turn_time)
if(TEMP):
manual_stop()
def forward(distance):
if (abs(distance) > 0.1):
AXLE_LENGTH = 0.22
WHEEL_RADIUS = 0.04
MIN_SPEED_MOTOR = 35
MAX_SPEED_MOTOR = 10000/18
MIN_SPEED = MIN_SPEED_MOTOR * 2 * math.pi / 60
MAX_SPEED = MAX_SPEED_MOTOR * 2 * math.pi / 60
FORWARD_SPEED = MIN_SPEED*2.5
if(distance < 0):
FORWARD_SPEED = MIN_SPEED
moving_time = abs(distance) /1.2 / WHEEL_RADIUS / FORWARD_SPEED
motor_left = 0
motor_right = 0
motor_left = speed_to_pwm(FORWARD_SPEED, MIN_SPEED, MAX_SPEED)
motor_right = speed_to_pwm(FORWARD_SPEED, MIN_SPEED, MAX_SPEED)
motor_speed = motorSpeed()
motor_speed.left_speed = motor_left
motor_speed.right_speed = motor_right
motor_speed.duration = moving_time
if(distance < 0):
if (debug):
rospy.loginfo("========================> MOVE BACKWARD: "+ str(round(moving_time, 2)))
motor_speed.left_speed = -motor_speed.left_speed
motor_speed.right_speed = -motor_speed.right_speed
else:
if (debug):
rospy.loginfo("========================> MOVE FORWARD: "+ str(round(moving_time, 2)))
motor_speed.duration = moving_time
if moving_time > 0:
motor_pub.publish(motor_speed)
time.sleep(moving_time)
if(TEMP):
manual_stop()
return 0
def move(angle, distance):
turn(angle)
forward(distance)
def sign(x):
if(x >= 0):
return 1
else:
return -1
# given a target position and a current position and angle
# returns the needed angle and distance to move
def position_to_movement(tpx, tpy, cpx, cpy, ca):
dy = tpy - cpy
dx = tpx - cpx
target_angle = math.atan2(dy,dx)
angle = target_angle - ca
distance = math.sqrt(dy*dy + dx*dx)
if (verbose):
rospy.loginfo("TARGET ANGLE="+str(round(target_angle, 2))+" ANGLE="+str(round(angle, 2))+" DIST="+str(round(distance, 2)))
return angle, distance
def get_localisation():
if (debug):
rospy.loginfo("WAITING...")
rospy.wait_for_service("compute_localisation")
if (debug):
rospy.loginfo("SERVICE OK")
try:
get_localisation = rospy.ServiceProxy('compute_localisation', GetLocalisation)
loca = get_localisation()
if (verbose):
rospy.loginfo("GOT LOCALISATION =====>"+ str(round(loca.pos_x, 2)) + " | " + str(round(loca.pos_y, 2)) + " | " + str(round(loca.angle, 2)))
return loca.pos_x, loca.pos_y, loca.angle
except rospy.ServiceException as e:
print("Service call failed: %s"%e)
# Returns true if the current position is close enough to the target position
def goal_reached(tpx, tpy, cpx, cpy, dist_threshold):
return ((abs(tpx-cpx) < dist_threshold) and
(abs(tpy-cpy) < dist_threshold))
def update_localisation(dist_threshold, counter):
manual_stop()
cpx, cpy, ca = get_localisation()
if (cpx < 0 or cpy < 0):
if (verbose):
rospy.loginfo("GOT BAD LOCALISATION")
if (dist_threshold < THRESHOLD and counter < 2):
if (verbose):
rospy.loginfo("TRYING AGAIN")
return update_localisation(dist_threshold, counter+1)
else:
return cpx, cpy, ca
else:
return cpx, cpy, ca
def wander(dist_threshold):
if (verbose):
rospy.loginfo("===========> WANDERING")
cpx = cpy = -1
while (cpx < 0 or cpy < 0):
forward(-dist_threshold)
cpx, cpy, ca = update_localisation(dist_threshold, 1)
if (verbose):
rospy.loginfo("WANDERING FINISHED <============")
return cpx, cpy, ca
def on_platform(px, py):
MIN_X_PLAT = 0
MAX_X_PLAT = 3
MIN_Y_PLAT = 0
MAX_Y_PLAT = 2
return (px >= MIN_X_PLAT and px <= MAX_X_PLAT) and (py >= MIN_Y_PLAT and py <= MAX_Y_PLAT)
def go_to_check(tpx, tpy, ta, dist_threshold, time_limit, prev_wp):
cpx, cpy, ca = update_localisation(dist_threshold, 0)
robot_on_platform = on_platform(cpx, cpy)
target_on_platform = on_platform(tpx, tpy)
on_plat = (robot_on_platform, target_on_platform)
wp = (tpx, tpy, ta, dist_threshold, time_limit)
if (robot_on_platform and not target_on_platform): # Needs to go down
way_points = down_slope_to
go_through_wps(way_points, time_limit)
elif (not robot_on_platform and target_on_platform): # Needs to go up
way_points = up_slope_to
go_through_wps(way_points, time_limit)
else:
if (ta is not None):
go_to_angle(tpx, tpy, ta, dist_threshold, time_limit)
else:
go_to(tpx, tpy, dist_threshold, time_limit)
def go_to(tpx, tpy, dist_threshold, time_limit):
MAX_DISTANCE = 0.5
MAX_BIG_DISTANCE = 2
MAX_PRECISE_DISTANCE = 0.3
MAX_ANGLE = math.pi/2
if(dist_threshold < THRESHOLD): # If we are doing precision work, use the precise max dist
MAX_DISTANCE = MAX_PRECISE_DISTANCE
elif(dist_threshold > THRESHOLD):
MAX_DISTANCE = MAX_BIG_DISTANCE
if (verbose):
rospy.loginfo("||||| USING BIG DISTANCE "+ round_two_dec(MAX_BIG_DISTANCE)+" |||||")
starting_time = time.time()
cpx, cpy, ca = update_localisation(dist_threshold, 0)
elapsed_time = time.time() - starting_time
while (not goal_reached(tpx, tpy, cpx, cpy, dist_threshold) and
elapsed_time < time_limit):
if (verbose):
rospy.loginfo("REMAINING TIME: "+ str(round(time_limit - elapsed_time, 2)))
if (cpx < 0 or cpy < 0):
cpx, cpy, ca = wander(dist_threshold)
else:
angle, distance = position_to_movement(tpx, tpy, cpx, cpy, ca)
if (verbose):
rospy.loginfo("TARGET LOCALISATION: "+ str(round(tpx, 2)) + " -- " + str(round(tpy, 2)))
rospy.loginfo("COMPUTED MOVEMENT (angle-dist): "+ str(angle) + " -- " + str(distance))
if (abs(angle) > MAX_ANGLE):
angle = MAX_ANGLE*sign(angle)
turn(angle)
else:
if (distance > MAX_DISTANCE):
distance = MAX_DISTANCE
move(angle, distance)
cpx, cpy, ca = update_localisation(dist_threshold, 0)
elapsed_time = time.time() - starting_time
if (verbose):
if (goal_reached(tpx, tpy, cpx, cpy, dist_threshold)):
rospy.loginfo("|||||||||||||| GOAL REACHED: "+round_two_dec(tpx)+";"+round_two_dec(tpy)+" |||||||||||||||||")
elif (elapsed_time >= time_limit):
rospy.loginfo("|||||||||||||| TIME's UP |||||||||||||||||")
return cpx, cpy, ca
def wander_angle(dist_threshold):
TURN_CONST = 0.05
if (verbose):
rospy.loginfo("===========> WANDERING FOR THE ANGLE")
cpx = cpy = -1
while (cpx < 0 or cpy < 0):
turn(TURN_CONST)
cpx, cpy, ca = update_localisation(dist_threshold, 1)
if (verbose):
rospy.loginfo("WANDERING FOR THE ANGLE FINISHED <============")
return cpx, cpy, ca
def check_n_turn(ta, cpx, cpy, ca, dist_threshold):
if (cpx < 0 or cpy < 0):
return wander_angle(dist_threshold)
else:
needed_angle = ta - ca
turn(needed_angle)
cpx, cpy, ca = update_localisation(dist_threshold, 0)
return cpx, cpy, ca
def go_to_angle(tpx, tpy, ta, dist_threshold, time_limit):
cpx, cpy, ca = go_to(tpx, tpy, dist_threshold, time_limit)
ANGLE_THRESHOLD = 0.1
MAX_TRIES = 5
nb_tries = 0
while (abs(ta-ca) > ANGLE_THRESHOLD and nb_tries < MAX_TRIES):
if (verbose):
rospy.loginfo("current and target angles: "+ str(round(ca, 2)) + " -- " + str(round(ta, 2)))
cpx, cpy, ca = check_n_turn(ta, cpx, cpy, ca, dist_threshold)
nb_tries += 1
if (verbose):
if (abs(ta-ca) < ANGLE_THRESHOLD):
rospy.loginfo("|||||||||||||| ANGLE REACHED: "+str(round(ta, 2))+"|||||||||||||||||")
else:
rospy.loginfo("|||||||||||||| TIME's UP ANGLE |||||||||||||||||")
return cpx, cpy, ca
def compute_timeout(tpx, tpy, cpx, cpy):
SEC_PER_METER = 15
CONST_TURN_TIME = 15
angle, distance = position_to_movement(tpx, tpy, cpx, cpy, 0)
return distance*SEC_PER_METER + CONST_TURN_TIME
def add_timeout_to_target_points(way_points, prev_wp):
target_points_with_to = []
for wp in way_points:
cpx = prev_wp[0]
cpy = prev_wp[1]
tpx = wp[0]
tpy = wp[1]
ta = wp[2]
timeout = compute_timeout(tpx, tpy, cpx, cpy)
precision_threshold = wp[3]
new_wp = (tpx, tpy, ta, precision_threshold, timeout)