461 lines
14 KiB
Lua
461 lines
14 KiB
Lua
|
-- Problem: Program si? wiesza przy zapisywaniu zdj?? i wysy?aniu requesta do API
|
||
|
|
http = require('socket.http')
|
||
|
|
json = require('json')
|
||
|
|
|
||
|
|
COLOR_WHITE = {255, 255, 255}
|
||
|
|
COLOR_RED = {255, 0, 0}
|
||
|
|
COLOR_GREEN = {0, 255, 0}
|
||
|
|
COLOR_BLUE = {0, 0, 255}
|
||
|
|
|
||
|
|
OBSTACLE_AVOID_DISTANCE = 3
|
||
|
|
MOVING_VELOCITY = 20
|
||
|
|
TURNING_VELOCITY = 15
|
||
|
|
PROXIMITY_STOP_DISTANCE = 1
|
||
|
|
CONFIDENCE = 0
|
||
|
|
IMAGE_COUNTER = 0
|
||
|
|
CAMERA_FIELD_OF_VIEW = 60
|
||
|
|
PHOTO_WIDTH = 1920
|
||
|
|
BATERY_VALUE = 10
|
||
|
|
|
||
|
|
function sysCall_init()
|
||
|
|
|
||
|
|
os.execute("rm -r robot")
|
||
|
|
os.execute("mkdir robot")
|
||
|
|
|
||
|
|
ModelHandle = sim.getObject(".")
|
||
|
|
ModelName = sim.getObjectAlias(ModelHandle)
|
||
|
|
|
||
|
|
RatHandles = {} -- List of rats on the scene
|
||
|
|
local i = 0
|
||
|
|
while true do
|
||
|
|
local rat_handle = sim.getObject("/Rat*", {index=i, noError=true})
|
||
|
|
if rat_handle < 0 then
|
||
|
|
break
|
||
|
|
end
|
||
|
|
table.insert(RatHandles, rat_handle)
|
||
|
|
i = i + 1
|
||
|
|
end
|
||
|
|
|
||
|
|
DuckHandles = {} -- List of ducks on the scene
|
||
|
|
local i = 0
|
||
|
|
while true do
|
||
|
|
local duck_handle = sim.getObject("/Duck*", {index=i, noError=true})
|
||
|
|
if duck_handle < 0 then
|
||
|
|
break
|
||
|
|
end
|
||
|
|
table.insert(DuckHandles, duck_handle)
|
||
|
|
i = i + 1
|
||
|
|
end
|
||
|
|
|
||
|
|
RightWheel = sim.getObject("./RightWheel")
|
||
|
|
LeftWheel = sim.getObject("./LeftWheel")
|
||
|
|
Camera = sim.getObject("./Camera")
|
||
|
|
VisionSensor = sim.getObject("./Vision_sensor")
|
||
|
|
SonarRight = sim.getObject("./Proximity_sensor_right")
|
||
|
|
SonarLeft = sim.getObject("./Proximity_sensor_left")
|
||
|
|
|
||
|
|
Diodes = { -- LED_strip has to be ungrouped
|
||
|
|
D2 = sim.getObject("./diode_2"),
|
||
|
|
D3 = sim.getObject("./diode_3"),
|
||
|
|
D4 = sim.getObject("./diode_4"),
|
||
|
|
D5 = sim.getObject("./diode_5"),
|
||
|
|
D6 = sim.getObject("./diode_6"),
|
||
|
|
D7 = sim.getObject("./diode_7")
|
||
|
|
}
|
||
|
|
|
||
|
|
sim.setShapeColor(sim.getObject("./diode_1"), nil, 0, COLOR_WHITE)
|
||
|
|
sim.setShapeColor(sim.getObject("./diode_8"), nil, 0, COLOR_WHITE)
|
||
|
|
|
||
|
|
sim.setObjectInt32Param(RightWheel,2000, 1) -- Enable velocity control
|
||
|
|
sim.setObjectInt32Param(LeftWheel,2000, 1)
|
||
|
|
sim.setObjectInt32Param(Camera,2000, 1)
|
||
|
|
|
||
|
|
Set_wheel_velocity(5, 5)
|
||
|
|
sim.setJointTargetVelocity(Camera, 0)
|
||
|
|
|
||
|
|
sim.setJointTargetForce(LeftWheel, 5)
|
||
|
|
sim.setJointTargetForce(RightWheel, 5)
|
||
|
|
|
||
|
|
Last_Color_Change_Time = sim.getSimulationTime()
|
||
|
|
Last_Camera_Rotation_Time = sim.getSimulationTime()
|
||
|
|
Last_Photo_Time = sim.getSimulationTime()
|
||
|
|
Last_Turn_Time=sim.getSimulationTime()
|
||
|
|
Turning = false
|
||
|
|
Seeing_Another_Rat = 0
|
||
|
|
Close_To_Rat = false
|
||
|
|
CameraAngle = Get_angle(VisionSensor)
|
||
|
|
ModelAngle = Get_angle(ModelHandle)
|
||
|
|
Rat_Degrees_Offset_From_Photo_Center = 0
|
||
|
|
|
||
|
|
DEBUG_Sensor_Time = sim.getSimulationTime()
|
||
|
|
|
||
|
|
--sim.addBanner("Bateria = 100", 2, sim_banner_fullyfacingcamera, nil, ModelHandle, nil, nil)
|
||
|
|
|
||
|
|
Camera_Rotate_Counter = 0
|
||
|
|
--IMAGE_COUNTER = 0
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
function sysCall_actuation()
|
||
|
|
-------------------- photo test --------------------
|
||
|
|
if (sim.getSimulationTime() - Last_Photo_Time) > 0.5 then
|
||
|
|
if BATERY_VALUE > 0 then
|
||
|
|
BATERY_VALUE = BATERY_VALUE - 1
|
||
|
|
UpdateBatery(BATERY_VALUE)
|
||
|
|
end
|
||
|
|
if not Close_To_Rat then
|
||
|
|
Take_photo()
|
||
|
|
end
|
||
|
|
Last_Photo_Time = sim.getSimulationTime()
|
||
|
|
end
|
||
|
|
|
||
|
|
-------------------- camera test -------------------
|
||
|
|
if (sim.getSimulationTime() - Last_Camera_Rotation_Time) > 1.7 then
|
||
|
|
if Camera_Rotate_Counter == 0 then
|
||
|
|
sim.setJointTargetVelocity(Camera, 2)
|
||
|
|
Camera_Rotate_Counter = 1
|
||
|
|
else
|
||
|
|
sim.setJointTargetVelocity(Camera, -2)
|
||
|
|
Camera_Rotate_Counter = 0
|
||
|
|
end
|
||
|
|
Last_Camera_Rotation_Time = sim.getSimulationTime()
|
||
|
|
end
|
||
|
|
|
||
|
|
-------------------- moving test -------------------
|
||
|
|
if Turning == true and Angle_difference(Get_angle(ModelHandle), Desired_angle) < 10 then
|
||
|
|
Finish_turning()
|
||
|
|
Stop_if_close(RatHandles)
|
||
|
|
end
|
||
|
|
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
function sysCall_sensing()
|
||
|
|
local _, _, _, detected_handle_left, _ = sim.readProximitySensor(SonarLeft)
|
||
|
|
local _, _, _, detected_handle_right, _ = sim.readProximitySensor(SonarRight)
|
||
|
|
if Has_Value(RatHandles, detected_handle_left) or Has_Value(RatHandles, detected_handle_right) then
|
||
|
|
if detected_handle_left > 0 and Has_Value(RatHandles, detected_handle_left) then
|
||
|
|
Handle = detected_handle_left
|
||
|
|
else
|
||
|
|
Handle = detected_handle_right
|
||
|
|
end
|
||
|
|
Close_To_Rat = true
|
||
|
|
Change_strip_color(COLOR_RED)
|
||
|
|
SensorMovement(Handle)
|
||
|
|
elseif BATERY_VALUE > 0 then
|
||
|
|
Close_To_Rat = false
|
||
|
|
VisionMovement()
|
||
|
|
else
|
||
|
|
Set_wheel_velocity(0, 0)
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
function sysCall_cleanup()
|
||
|
|
-- do some clean-up here
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
------------------------------ non-sysCall functions below ------------------------------
|
||
|
|
function VisionMovement()
|
||
|
|
if Seeing_Another_Rat == 1 then
|
||
|
|
local adjusted_angle = Calculate_desired_angle(CameraAngle, Rat_Degrees_Offset_From_Photo_Center)
|
||
|
|
local angle_diff = Angle_difference(ModelAngle, adjusted_angle)
|
||
|
|
if angle_diff > 5 then
|
||
|
|
if math.abs(ModelAngle - adjusted_angle) < 180 then
|
||
|
|
if ModelAngle < adjusted_angle then
|
||
|
|
print(string.format("[%s] Rat detected on the left", ModelName))
|
||
|
|
else
|
||
|
|
print(string.format("[%s] Rat detected on the right", ModelName))
|
||
|
|
angle_diff = -angle_diff
|
||
|
|
end
|
||
|
|
else
|
||
|
|
if ModelAngle > adjusted_angle then
|
||
|
|
print(string.format("[%s] Rat detected on the left", ModelName))
|
||
|
|
else
|
||
|
|
print(string.format("[%s] Rat detected on the right", ModelName))
|
||
|
|
angle_diff = -angle_diff
|
||
|
|
end
|
||
|
|
end
|
||
|
|
DEGREES_TO_TURN = angle_diff
|
||
|
|
Start_turning(angle_diff)
|
||
|
|
ModelAngle = 0
|
||
|
|
CameraAngle = 0
|
||
|
|
Rat_Degrees_Offset_From_Photo_Center = 0
|
||
|
|
end
|
||
|
|
if not Turning then
|
||
|
|
Go_Forward()
|
||
|
|
end
|
||
|
|
elseif not Turning then
|
||
|
|
Random_Walk()
|
||
|
|
end
|
||
|
|
if not Turning then
|
||
|
|
Stop_if_close(RatHandles)
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|
||
|
|
function SensorMovement(handle)
|
||
|
|
Follow(handle)
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
function RatCallPredictionAPI(filename)
|
||
|
|
local response = http.request(string.format("http://127.0.0.1:5000/detectRat1?img=%s", filename))
|
||
|
|
return json.decode(response)
|
||
|
|
end
|
||
|
|
|
||
|
|
function RobotCallPredictionAPI(filename)
|
||
|
|
local response = http.request(string.format("http://127.0.0.1:5000/detectRobot1?img=%s", filename))
|
||
|
|
return json.decode(response)
|
||
|
|
end
|
||
|
|
|
||
|
|
function UpdateBatery(batery_value)
|
||
|
|
sim.addBanner(batery_value, 0.4, sim_banner_fullyfacingcamera, {1.0, 0, 0, 0, 0, 0}, ModelHandle, nil, {1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0})
|
||
|
|
end
|
||
|
|
|
||
|
|
function Has_Value (tab, val)
|
||
|
|
for index, value in ipairs(tab) do
|
||
|
|
if value == val then
|
||
|
|
return true
|
||
|
|
end
|
||
|
|
end
|
||
|
|
return false
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
function Change_strip_color(rgb_vals)
|
||
|
|
for _, v in pairs(Diodes) do
|
||
|
|
sim.setShapeColor(v, nil, 0, rgb_vals)
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
function Set_wheel_velocity(left_vel, right_vel)
|
||
|
|
sim.setJointTargetVelocity(LeftWheel, left_vel)
|
||
|
|
sim.setJointTargetVelocity(RightWheel, right_vel)
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
function Take_photo()
|
||
|
|
CameraAngle = Get_angle(VisionSensor)
|
||
|
|
ModelAngle = Get_angle(ModelHandle)
|
||
|
|
local img = sim.getVisionSensorImg(VisionSensor)
|
||
|
|
local filename = "img"..IMAGE_COUNTER..".png"
|
||
|
|
local path = "robot1/"..filename
|
||
|
|
sim.saveImage(img, {1920,1080}, 0, path, -1)
|
||
|
|
print(string.format("[%s] Photo taken 1", ModelName))
|
||
|
|
local res = RatCallPredictionAPI(filename)
|
||
|
|
local res_robot = RobotCallPredictionAPI(filename)
|
||
|
|
print(CONFIDENCE)
|
||
|
|
if BATERY_VALUE > 0 then
|
||
|
|
if table.concat(res['0']) == 'None' then
|
||
|
|
--print(CONFIDENCE)
|
||
|
|
if CONFIDENCE > 0 then
|
||
|
|
CONFIDENCE = CONFIDENCE - 1
|
||
|
|
end
|
||
|
|
Seeing_Another_Rat = 0
|
||
|
|
if not Close_To_Rat and CONFIDENCE == 0 then
|
||
|
|
Change_strip_color(COLOR_GREEN)
|
||
|
|
print(string.format("[%s] No rats detected!", ModelName))
|
||
|
|
else
|
||
|
|
print(string.format("[%s] Chase the rat!", ModelName))
|
||
|
|
end
|
||
|
|
|
||
|
|
else
|
||
|
|
local frame_width = res['0'][1]
|
||
|
|
local rat_pos = res['0'][2] + frame_width / 2
|
||
|
|
Rat_Degrees_Offset_From_Photo_Center = (rat_pos - (PHOTO_WIDTH / 2)) / (PHOTO_WIDTH/CAMERA_FIELD_OF_VIEW)
|
||
|
|
if table.concat(res_robot['0']) == 'None' then
|
||
|
|
Seeing_Another_Rat = 1
|
||
|
|
Change_strip_color(COLOR_RED)
|
||
|
|
CONFIDENCE = 3
|
||
|
|
print(string.format("[%s] Another rat detected on camera", ModelName))
|
||
|
|
elseif res_robot['0'][1] == 'red' then
|
||
|
|
Seeing_Another_Rat = 0
|
||
|
|
print(string.format("[%s] I'm looking for another rat. This robot doesn't need my help", ModelName))
|
||
|
|
if CONFIDENCE == 0 then
|
||
|
|
Change_strip_color(COLOR_GREEN)
|
||
|
|
end
|
||
|
|
|
||
|
|
elseif res_robot['0'][1] == 'white' then
|
||
|
|
Seeing_Another_Rat = 1
|
||
|
|
print(string.format("[%s] Robot need my help. I'm on my way!", ModelName))
|
||
|
|
Change_strip_color(COLOR_RED)
|
||
|
|
CONFIDENCE = 3
|
||
|
|
end
|
||
|
|
end
|
||
|
|
IMAGE_COUNTER = IMAGE_COUNTER + 1
|
||
|
|
else
|
||
|
|
Change_strip_color(COLOR_WHITE)
|
||
|
|
print(string.format("[%s] I have to charge my battery", ModelName))
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
function Orientation_to_angle(orientation)
|
||
|
|
-- returns a value between [0, 360]
|
||
|
|
local angle = math.deg(orientation[2]) + 90
|
||
|
|
if orientation[1] < 0 then
|
||
|
|
angle = 360 - angle
|
||
|
|
end
|
||
|
|
return angle
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
function Get_angle(handle)
|
||
|
|
local orientation = sim.getObjectOrientation(handle, sim.handle_world)
|
||
|
|
return Orientation_to_angle(orientation)
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
function Calculate_desired_angle(old_angle, angle_delta)
|
||
|
|
local desired = old_angle + angle_delta
|
||
|
|
if desired > 360 then
|
||
|
|
return desired % 360
|
||
|
|
elseif
|
||
|
|
desired < 0 then
|
||
|
|
return desired + 360
|
||
|
|
else
|
||
|
|
return desired
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
function Angle_difference(actual_angle, desired_angle)
|
||
|
|
-- returns a value between [0, 180]
|
||
|
|
local diff = math.abs(desired_angle - actual_angle)
|
||
|
|
return diff < 180 and diff or 360 - diff
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
function Turn_Left()
|
||
|
|
Set_wheel_velocity(0, TURNING_VELOCITY)
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
function Turn_Right()
|
||
|
|
Set_wheel_velocity(TURNING_VELOCITY,0)
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
function Go_Forward()
|
||
|
|
Set_wheel_velocity(MOVING_VELOCITY, MOVING_VELOCITY)
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
function Stop_Moving()
|
||
|
|
Set_wheel_velocity(0, 0)
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
function Start_turning(degrees)
|
||
|
|
FULL_CIRCLE_PERIOD = 2.8 -- at 2.5 speed
|
||
|
|
print(string.format("[%s] Starting to turn %.2f degrees", ModelName, degrees))
|
||
|
|
Turning = true
|
||
|
|
Desired_angle = Calculate_desired_angle(Get_angle(ModelHandle), degrees)
|
||
|
|
if degrees > 0 then
|
||
|
|
Turn_Left()
|
||
|
|
else
|
||
|
|
Turn_Right()
|
||
|
|
end
|
||
|
|
Last_Turn_Time = sim.getSimulationTime()
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
function Finish_turning()
|
||
|
|
Go_Forward()
|
||
|
|
Last_Turn_Time = sim.getSimulationTime()
|
||
|
|
Turning = false
|
||
|
|
After_turning = Get_angle(ModelHandle)
|
||
|
|
print(string.format("[%s] Finished turning; Angle deviation from desired: %.2f (Desired: %.2f vs Actual: %.2f)",
|
||
|
|
ModelName,
|
||
|
|
Angle_difference(After_turning, Desired_angle),
|
||
|
|
Desired_angle,
|
||
|
|
After_turning))
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
function Random_Walk()
|
||
|
|
local _, obstacle_left_distance, _, _, _ = sim.readProximitySensor(SonarLeft)
|
||
|
|
if obstacle_left_distance == 0 then
|
||
|
|
obstacle_left_distance = math.huge
|
||
|
|
end
|
||
|
|
|
||
|
|
local _, obstacle_right_distance, _, _, _ = sim.readProximitySensor(SonarRight)
|
||
|
|
if obstacle_right_distance == 0 then
|
||
|
|
obstacle_right_distance = math.huge
|
||
|
|
end
|
||
|
|
|
||
|
|
if obstacle_right_distance < obstacle_left_distance and obstacle_right_distance < OBSTACLE_AVOID_DISTANCE then
|
||
|
|
Turn_Left()
|
||
|
|
elseif obstacle_left_distance < obstacle_right_distance and obstacle_left_distance < OBSTACLE_AVOID_DISTANCE then
|
||
|
|
Turn_Right()
|
||
|
|
else
|
||
|
|
Go_Forward()
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
function Follow(object_handle) -- Use with flat sensors with no blind zone in the middle
|
||
|
|
local left_detected, left_distance, _, _, _ = sim.checkProximitySensor(SonarLeft, object_handle)
|
||
|
|
local right_detected, right_distance, _, _, _ = sim.checkProximitySensor(SonarRight, object_handle)
|
||
|
|
|
||
|
|
if (sim.getSimulationTime() - Last_Photo_Time) > 0.5 then
|
||
|
|
if not Close_To_Rat then
|
||
|
|
local img = sim.getVisionSensorImg(VisionSensor)
|
||
|
|
local filename = "img"..IMAGE_COUNTER..".png"
|
||
|
|
local path = "robot1/"..filename
|
||
|
|
sim.saveImage(img, {1920,1080}, 0, path, -1)
|
||
|
|
end
|
||
|
|
Last_Photo_Time = sim.getSimulationTime()
|
||
|
|
IMAGE_COUNTER = IMAGE_COUNTER + 1
|
||
|
|
end
|
||
|
|
|
||
|
|
if left_detected == 1 and right_detected == 1 then
|
||
|
|
Go_Forward()
|
||
|
|
elseif left_detected == 1 then
|
||
|
|
Turn_Left()
|
||
|
|
elseif right_detected == 1 then
|
||
|
|
Turn_Right()
|
||
|
|
else
|
||
|
|
Go_Forward()
|
||
|
|
end
|
||
|
|
|
||
|
|
if (left_distance > 0 and left_distance < PROXIMITY_STOP_DISTANCE) or (right_distance > 0 and right_distance < PROXIMITY_STOP_DISTANCE) then
|
||
|
|
Stop_Moving()
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
function Stop_if_close(handle_table)
|
||
|
|
-- Przy szczurach jest ok przy robotach z jakiegos powodu nie wykrywa jesli patrzy na kolo zamiast chassisa (kolo pewnie ma inny objecthandle?)
|
||
|
|
local left_detected, left_distance, _, left_detected_object_handle, _ = sim.readProximitySensor(SonarLeft)
|
||
|
|
local right_detected, right_distance, _, right_detected_object_handle, _ = sim.readProximitySensor(SonarRight)
|
||
|
|
if Has_Value(handle_table, left_detected_object_handle) or Has_Value(handle_table, right_detected_object_handle) then
|
||
|
|
if (left_distance > 0 and left_distance < PROXIMITY_STOP_DISTANCE) or (right_distance > 0 and right_distance < PROXIMITY_STOP_DISTANCE) then
|
||
|
|
Stop_Moving()
|
||
|
|
end
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
------------------------------ DEBUG functions below ------------------------------
|
||
|
|
function DEBUG_What_Do_I_See(sensor_handle)
|
||
|
|
local detected, distance, detected_point, detected_object_handle, detected_surface_normal_vector = sim.readProximitySensor(sensor_handle)
|
||
|
|
if detected == 1 then
|
||
|
|
print(string.format("[%s] Object detected by %s; Handle: %d, Name: %s, Distance: %.2f",
|
||
|
|
ModelName,
|
||
|
|
sim.getObjectAlias(sensor_handle),
|
||
|
|
detected_object_handle,
|
||
|
|
sim.getObjectAlias(detected_object_handle, 1),
|
||
|
|
distance))
|
||
|
|
end
|
||
|
|
end
|