Bonjour à tous, je travaille sur un projet actuellement et j'aimerais savoir comment modifier le code ci dessous afin d'obtenir la pose (position + orientation) d'un robot en centimètres (ou mètres). J'ai une effectué un tracé de trajectoire à partir de la carte et j'obtiens des valeurs en pixel.
Voici le code que j'ai trouvé sur un autre projet, mais celui-ci ne correspond pas à mon projet:
Merci d'avance
Code : Sélectionner tout - Visualiser dans une fenêtre à part
1
2
3
4
5
6
7
8
9
10
11
12 def pose_to_pix(self, pose_robot): w = self.metadata[0] h = self.metadata[1] res = self.metadata[2] # We first convert pose_robot from the "map" frame to the image frame x_robot, y_robot,theta_robot = pose_robot x_origin, y_origin, theta_origin= self.pose_origin xr_in_im = (y_origin - y_robot) / res + h yr_in_im = (x_robot - x_origin) / res # And apply a rotation theta_in_im = theta_robot - theta_origin return (int(xr_in_im), int(yr_in_im), theta_in_im)
Partager