Getting rotation angle from laser data

asked 2021-08-09 19:14:18 -0500

sisko gravatar image

updated 2021-08-10 04:47:31 -0500

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.

image description

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
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 ?

edit retag flag offensive close merge delete


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?

So Young gravatar image So Young  ( 2021-08-09 23:32:58 -0500 )edit

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?

ijnek gravatar image ijnek  ( 2021-08-10 00:27:05 -0500 )edit

@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.

sisko gravatar image sisko  ( 2021-08-10 04:52:59 -0500 )edit

@ijnek: I updated the question to include the code I use for rotation.

sisko gravatar image sisko  ( 2021-08-10 04:53:52 -0500 )edit

@So Young: CORRECTION. I'm trying to calculate the angle using Adjacent/Hypothenus

sisko gravatar image sisko  ( 2021-08-10 05:07:37 -0500 )edit

In your code, you have

sine = math.sin( median/self.regions['starboard_abeam_bow'][0] )

doesnt seem right. math.sin(θ) should take an angle θ. In your case, I think it should be

heading_error = math.acos(median/self.regions['starboard_abeam_bow'][0] )
ijnek gravatar image ijnek  ( 2021-08-10 08:38:43 -0500 )edit

Ransac, line detection representing the wall from the lidar points, get the relative angle from the line coefficients and adjust robot pose.

Mehdi. gravatar image Mehdi.  ( 2021-08-10 08:44:31 -0500 )edit

@mehdi: Can you please explain more ?

sisko gravatar image sisko  ( 2021-08-10 08:46:59 -0500 )edit