Getting rotation angle from laser data
I need help finding the exact angle to rotate my robot so that it is directly facing an obstacle.
My thought process is to extract information from my laser readings which represent horizontal and diagonal distance to the obstacle, as illustrated below.
To get the "green" distance I used the data in the first index of the right laser. For the "red" distance, I used the median value of the right laser.
I'm using the following to calculate the angle:
self.kP = 0.5
.
.
sine = math.sin( median/self.regions['starboard_abeam_bow'][0] )
target = sine * math.pi/180 # converting degrees into radians
if format(target, '.2f') == format(self.yaw, '.2f') :
move.angular.z = 0
rospy.logerr('Target Acuired!')
self.target_acquired = True
exit()
else :
move.angular.z = self.kP * (target - self.yaw)
rospy.loginfo('Target: %s, Yaw: %s', target, self.yaw)
However, my robot does not rotate.
If my process is wrong, can I get some advice on how to get the angle information ? And, is there a way I can visualize both lines to confirm the triangle I am working with ?
I'm not sure if I understood the diagram right, but isn't cosine = median/laser_right? and to get the theta angle , use arccos? let me know if I understood it worng. Also, have you checked your laser_right[0] and median_valuse has appropriate value?
In the code you provided, there is no code to make the robot "rotate". There is only code to stop the rotation. Would you be able to post where you're setting anglar.z to a value that is not zero?
@So Young: I'm trying to use Oppsite/Hypotenus to get the angle. Regarding checking the values, yes there is data in the values being passed into my code but I do wonder if merely using the value in the first index is correct. That's why in the second part of my question I asked if it's possible to "view" the laser somehow in gazebo to confirm. I updated my code.
@ijnek: I updated the question to include the code I use for rotation.
@So Young: CORRECTION. I'm trying to calculate the angle using Adjacent/Hypothenus
In your code, you have
doesnt seem right. math.sin(θ) should take an angle θ. In your case, I think it should be
Ransac, line detection representing the wall from the lidar points, get the relative angle from the line coefficients and adjust robot pose.
@mehdi: Can you please explain more ?