def goto(self):
# theta = self.quaternion_to_euler()
theta = 1
# Deplace le robot jusqu'au point voulu
goal_pose = PoseWithCovarianceStamped()
# recuperation des informations de l'utilisateur
goal_pose.pose.point.x = input("Rentrez la position en x :")
goal_pose.y = input("Rentrez la position en y :")
goal_pose.theta = input("Rentrez l'angle :")
# Definition tolerance --> gestion de l'espace proche
distance_tolerance = 0.1
speed_msg = Twist()
print("Position du robot: x {}, y {}, theta {}".format(
self.pose.pose.pose.point.x,
self.pose.pose.pose.point.y,
theta,
))
print("Lancement...")
while self.distance(goal_pose) >= distance_tolerance:
print("En cours")
# Affiche les positions :
#
print("Position du robot en x : {}, y : {} theta: {}\n".format(
self.pose.pose.pose.point.x,
self.pose.pose.pose.point.y,
theta,
))
print("Position souhaitee : x {}, y {}, theta {}\n".format(
goal_pose.x,
goal_pose.y,
goal_pose.theta,
))
# Proportionnel controle
# Vitesse linaire en x
speed_msg.linear.x = self.line_speed(goal_pose)
speed_msg.linear.y = 0
speed_msg.linear.z = 0
# Vitesse angulaire en z
speed_msg.angular.x = 0
speed_msg.angular.y = 0
speed_msg.angular.z = self.angular_speed(goal_pose)
# Edition notre vitesse
self.velocity_publisher.publish(speed_msg)
# Edition attente
self.rate.sleep()
# Arret du robot une fois que le point est atteint
speed_msg.linear.x = 0
speed_msg.angular.z = 0
self.velocity_publisher.publish(speed_msg)
# Gestion de l'annulation du programme (crl+c)
rospy.spin()
