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()