Commit dececa06 authored by Agustin Rieppi Espasandin's avatar Agustin Rieppi Espasandin
Browse files

Find mean dealy

parent 302c3bad
......@@ -124,7 +124,7 @@ class GazeboConnection():
We initialise the physics parameters of the simulation, like gravity,
friction coeficients and so on.
"""
self._time_step = Float64(0.0073)
self._time_step = Float64(0.002)
self._max_update_rate = Float64(0)
self._gravity = Vector3()
......
......@@ -11,7 +11,8 @@ from gazebo_msgs.srv import SetModelState
from geometry_msgs.msg import Point, Vector3
from scipy.spatial.transform import Rotation
from sailboat_controller.srv import GetSpeed, GetSpeedResponse
import math
import random
class SailboatEnv(robot_gazebo_env.RobotGazeboEnv):
def __init__(self):
self.controllers_list = []
......@@ -31,7 +32,10 @@ class SailboatEnv(robot_gazebo_env.RobotGazeboEnv):
self.current = Vector3(0,0,0)
self.wind_current = Vector3(0,0,0)
self.wait_time = 0
self.messures = 0
#print("Registrandooo")
#self.pub_wind_current = rospy.Service('/windCurrent', GetSpeed, self.handleWindCurrent)
def _check_all_systems_ready(self):
......@@ -85,14 +89,76 @@ class SailboatEnv(robot_gazebo_env.RobotGazeboEnv):
## Make actions ##
def move_joints(self, actions):
actions[0] = round(random.uniform(-1.57, 1.57), 4)
actions[1] = round(random.uniform(-3.14, 3.14), 4)
msg = JointState()
msg.header = Header()
msg.name = ['rudder_joint', 'sail_joint']
msg.position = [actions[0], actions[1]]
msg.velocity = []
msg.effort = []
self.pub_joint.publish(msg)
rospy.sleep(0.5)
res = self.wait_for_move(actions)
if res < 2:
self.messures = self.messures + 1
print("Messure: ", self.messures)
self.wait_time = self.wait_time + res
print(res)
if self.messures > 500:
media = self.wait_time/self.messures
print("Media: ", media)
exit(1)
def check_action_ready(self):
action = None
while action is None and not rospy.is_shutdown():
try:
action = rospy.wait_for_message("/sailboat/joint_states", JointState, timeout=5.0)
except:
rospy.logerr("Current /odom not ready yet, retrying for getting odom")
return action
def wait_for_move(self, action):
rudder_epsilon = 0.03
sail_epsilon = 0.3
start_wait_time = rospy.get_rostime().to_sec()
end_wait_time=0
rudder_pos = action[0]
sail_pos = action[1]
rudder_pos_plus = rudder_pos + rudder_epsilon
rudder_pos_minus = rudder_pos - rudder_epsilon
sail_pos_plus = sail_pos + sail_epsilon
sail_pos_minus = sail_pos - sail_epsilon
while not rospy.is_shutdown():
action_state = self.check_action_ready()
rudder = action_state.position[0]
sail = action_state.position[1]
rudder_are_close = (rudder <= rudder_pos_plus) and (rudder > rudder_pos_minus)
sail_are_close = (sail <= sail_pos_plus) and (sail > sail_pos_minus)
#d = abs(rudder - rudder_pos)
#print("Rudder distnac: ", d)
#a = abs(sail_pos - sail)
#print("Sail distnac: ", a)
if rudder_are_close and sail_are_close:
end_wait_time = rospy.get_rostime().to_sec()
break
if (rospy.get_rostime().to_sec() - start_wait_time) > 5:
end_wait_time = rospy.get_rostime().to_sec()
break
rospy.sleep(0.01)
delta_time = end_wait_time - start_wait_time
return delta_time
def _check_joint_states_ready(self):
joint_states = None
......@@ -109,8 +175,8 @@ class SailboatEnv(robot_gazebo_env.RobotGazeboEnv):
state_msg = ModelState()
state_msg.model_name = 'sailboat'
state_msg.pose.position.x = 240
state_msg.pose.position.y = 95
state_msg.pose.position.x = 0
state_msg.pose.position.y = 0
state_msg.pose.position.z = 0
state_msg.pose.orientation.x = rot_quat[0]
state_msg.pose.orientation.y = rot_quat[1]
......
......@@ -55,7 +55,7 @@ class MyTrainingEnv(sailboat_env.SailboatEnv):
self.max_rudder = +1.57
self.min_rudder = -1.57
self.max_sail = +3.14
self.min_sail = -3.14
self.min_sail = -3.140073
self.action_space = spaces.Box(
np.float32([-1, -1]),
......@@ -203,7 +203,7 @@ class MyTrainingEnv(sailboat_env.SailboatEnv):
if (self.environment_type == 'OPTUNA'):
max_steps = 150
else:
max_steps = 500
max_steps = 10500
if self.cumulated_steps > max_steps:
self._episode_done = True
if DEBUG_EPISODE_FAIL:
......
......@@ -13,11 +13,12 @@ class SailboatUtils():
random.seed(1)
###
self.expert_waypoints = [
(240.0, 130.0, 0.0), # traves(+0, +35)
(275.0, 95.0, 0.0), # popa(+35,+0)
(275.0, 95.0, 0.0), # popa(al reves)
(270.0, 113.0, 0.0), # (izq) aleta(+30, +18)
(258.0, 125.0, 0.0), # largo(+18, +30)
(240.0, 130.0, 0.0), # traves(+0, +35)
(215.0, 70.0, 0.0), # (der) ceñida(-25, -25 )
(225.0, 63.0, 0.0), # descuartelar(-15, -32)
(240.0, 60.0, 0.0), # traves(+0, -35)
......
Markdown is supported
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