def l1(x, y):
return math.sqrt(x**2, y**2)
def yaw(euler_angles):
return euler_angles[2]
def dist(matrix):
return np.linalg.norm([matrix[0][3], matrix[1][3], matrix[2][3]])
steering_angle = -math.pi
while True:
lane_pose = env.get_lane_pos2(env.cur_pos, env.cur_angle)
distance_to_road_center = lane_pose.dist
angle_from_straight_in_rads = lane_pose.angle_rad
###### Start changing the code here.
# TODO: Decide how to calculate the speed and direction.
k_p = 10
k_d = 1
# The speed is a value between [0, 1] (which corresponds to a real speed between 0m/s and 1.2m/s)
speed = 0.2 # TODO: You should overwrite this value
# angle of the steering wheel, which corresponds to the angular velocity in rad/s
steering = k_p * distance_to_road_center + k_d * angle_from_straight_in_rads # TODO: You should overwrite this value
return euler_angulos[2]
def dist(matrix):
return np.linalg.norm([matrix[0][3],matrix[1][3],matrix[2][3]])
direccion_angulo=-math.pi
nro_iter = 0
# se define si se sigue al apriltag o no
seguir_april = True
dist_april = 1
while True:
nro_iter = nro_iter + 1
pose_del_pato = env.get_lane_pos2(env.cur_pos, env.cur_angle)
distancia_de_manejo = pose_del_pato.dist
angulo_recto = pose_del_pato.angle_rad
k_p = 10
k_d = 1
speed = 0.2
# angulo del volante, que corresponde a la velocidad angular en rad/s
direccion = k_p*distancia_de_manejo + k_d*angulo_recto
obs, reward, done, info = env.step([speed, direccion_angulo])
# Correccion de la imagen externa del pato
original = Image.fromarray(obs)
original = np.ascontiguousarray(np.flip(original, axis=0))
class DuckiebotSim(object):
def __init__(self):
# Initialize Robot Simulation
self.env = DuckietownEnv( seed=1, map_name = 'udem1', draw_curve = False, draw_bbox = False, distortion = False, domain_rand = False)
self.env.reset()
self.env.render()
# self.action = np.array([0.44, 0.0])
self.action = np.array([0.0, 0.0])
# For image to ros
self.bridge = CvBridge()
# Initialize ROS Node
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" %(self.node_name))
# Setup parameters
self.framerate = self.setupParam("~framerate",self.env.unwrapped.frame_rate)
# Setup Publisher
self.pub_img= rospy.Publisher("image_raw",Image,queue_size=1)
self.has_published = False
# Setup Subscriber
#self.sub_cmd = rospy.Subscriber("/cmd_vel", Twist, self.cbCmd, queue_size=1)
# Setup timer
self.timer_img_low = rospy.Timer(rospy.Duration.from_sec(1.0/self.framerate),self.cbTimer)
rospy.loginfo("[%s] Initialized." %(self.node_name))
def setupParam(self,param_name,default_value):
value = rospy.get_param(param_name,default_value)
rospy.set_param(param_name,value) #Write to parameter server for transparancy
rospy.loginfo("[%s] %s = %s " %(self.node_name,param_name,value))
return value
def cbTimer(self,event):
if not rospy.is_shutdown():
self.grabAndPublish(self.pub_img)
def grabAndPublish(self,publisher):
self.action = self.basic_control()
# Grab image from simulation and apply the action
obs, reward, done, info = self.env.step(self.action)
#rospy.loginfo('[%s] step_count = %s, reward=%.3f' % (self.node_name, self.env.unwrapped.step_count, reward))
image = cv2.cvtColor(obs, cv2.COLOR_BGR2RGB) # Correct color for cv2
# Publish raw image
image_msg = self.bridge.cv2_to_imgmsg(image)
image_msg.header.stamp = rospy.Time.now()
# Publish
publisher.publish(image_msg)
if not self.has_published:
rospy.loginfo("[%s] Published the first image." %(self.node_name))
self.has_published = True
# if done and (self.env.unwrapped.step_count != 1500):
# rospy.logwarn("[%s] DONE! RESETTING." %(self.node_name))
# self.env.reset()
#self.env.render()
def cbCmd(self,cmd_msg):
linear_vel = cmd_msg.linear.x # Forward Velocity [-1 1]
angular_vel = cmd_msg.angular.z # Steering angle [-1 1]
self.action = np.array([linear_vel, angular_vel])
def onShutdown(self):
rospy.loginfo("[%s] Shutdown." %(self.node_name))
self.env.close()
def basic_control(self):
lane_pose = self.env.get_lane_pos2(self.env.cur_pos, self.env.cur_angle)
distance_to_road_center = lane_pose.dist
angle_from_straight_in_rads = lane_pose.angle_rad
rospy.loginfo(" dist = %.2f" %(distance_to_road_center))
rospy.loginfo("theta = %.2f" %(angle_from_straight_in_rads))
k_p = 10
k_d = 5
speed = 0.2
steering = k_p*distance_to_road_center + k_d*angle_from_straight_in_rads
return [speed, steering]
class DuckiebotSim(object):
def __init__(self):
# Initialize Robot Simulation
self.env = DuckietownEnv(seed=1,
map_name='udem1',
draw_curve=False,
draw_bbox=False,
distortion=False)
self.env.reset()
self.env.render()
# self.action = np.array([0.44, 0.0])
self.action = np.array([0.0, 0.0])
# For image to ros
self.bridge = CvBridge()
# Initialize ROS Node
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing......" % (self.node_name))
# Setup parameters
self.framerate = self.setupParam("~framerate",
self.env.unwrapped.frame_rate)
# Setup Publisher
self.pub_img = rospy.Publisher("image_raw", Image, queue_size=1)
self.has_published = False
# Setup Subscriber
self.sub_cmd = rospy.Subscriber("/cmd_vel",
Twist,
self.cbCmd,
queue_size=1)
self.count = 0
self.outF = open("data.csv", 'w')
# Setup timer
self.timer_img_low = rospy.Timer(
rospy.Duration.from_sec(1.0 / self.framerate), self.cbTimer)
rospy.loginfo("[%s] Initialized." % (self.node_name))
def setupParam(self, param_name, default_value):
value = rospy.get_param(param_name, default_value)
rospy.set_param(param_name,
value) #Write to parameter server for transparancy
rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
return value
def cbTimer(self, event):
if not rospy.is_shutdown():
self.grabAndPublish(self.pub_img)
def grabAndPublish(self, publisher):
# Grab image from simulation and apply the action
obs, reward, done, info = self.env.step(self.action)
#rospy.loginfo('[%s] step_count = %s, reward=%.3f' % (self.node_name, self.env.unwrapped.step_count, reward))
image = cv2.cvtColor(obs, cv2.COLOR_BGR2RGB) # Correct color for cv2
"""
##show image
cv2.namedWindow("Image")
if (not image is None):
cv2.imshow("Image",image)
if cv2.waitKey(1)!=-1: #Burak, needs to modify this line to work on your computer, THANKS!
cv2.destroyAllWindows()
"""
# Publish raw image
image_msg = self.bridge.cv2_to_imgmsg(image)
image_msg.header.stamp = rospy.Time.now()
# Publish
publisher.publish(image_msg)
if not self.has_published:
rospy.loginfo("[%s] Published the first image." % (self.node_name))
self.has_published = True
if (self.env.unwrapped.step_count >= 200):
lane_pose = self.env.get_lane_pos2(self.env.cur_pos,
self.env.cur_angle)
distance_to_road_center = lane_pose.dist
angle_from_straight_in_rads = lane_pose.angle_rad
print(self.count)
print(
"Distance to Center: {:.2f}".format(distance_to_road_center))
print("Angle from Straight: {:.2f}".format(
angle_from_straight_in_rads))
self.outF.write("{:d},{:.2f},{:.2f}\n".format(
self.count, distance_to_road_center,
angle_from_straight_in_rads))
# Save the image
filename = "{:d}".format(self.count)
cv2.imwrite('{:s}.png'.format(filename), image)
self.count += 1
#rospy.logwarn("[%s] DONE! RESETTING." %(self.node_name))
self.env.reset()
self.env.render()
def cbCmd(self, cmd_msg):
linear_vel = cmd_msg.linear.x # Forward Velocity [-1 1]
angular_vel = cmd_msg.angular.z # Steering angle [-1 1]
self.action = np.array([linear_vel, angular_vel])
def onShutdown(self):
self.outF.close()
rospy.loginfo("[%s] Shutdown." % (self.node_name))
self.env.close()
