/odom topic problem in Real Robot Lab

Course Support ROS2 Basics in 5 Days Humble (Python)
1

Hi,

I am working with the Real Robot for my Wall Follower rosject and I found an error with the /odom topic. I don’t know if this is due to my code or hardware. What happens is that even when the robot is still, the values published on the /odom topic change.

My code for the odom_callback function is the following:

def odom_callback(self, msg):
    x = msg.pose.pose.orientation.x
    y = msg.pose.pose.orientation.x
    z = msg.pose.pose.orientation.z
    w = msg.pose.pose.orientation.w
    self.get_logger().info("(" + str(x) + ", " + str(y) + ", " + str(z) + ", " + str(w) + ")")
    _, _, yaw = self.euler_from_quaternion([x, y, z, w])
    ...

What I get printed from this function:

[wall_finder-1] [INFO] [1678735063.178043223] [wall_finder_server]: (0.0, 0.0, 0.44142162799835205, 0.8972998261451721)
[wall_finder-1] [INFO] [1678735063.277297594] [wall_finder_server]: (0.0, 0.0, 0.4350792467594147, 0.9003921747207642)
[wall_finder-1] [INFO] [1678735063.296599984] [wall_finder_server]: (0.0, 0.0, 0.4320705831050873, 0.9018397927284241)
[wall_finder-1] [INFO] [1678735063.376826597] [wall_finder_server]: (0.0, 0.0, 0.42722752690315247, 0.9041441679000854)
[wall_finder-1] [INFO] [1678735063.378956432] [wall_finder_server]: (0.0, 0.0, 0.4242747128009796, 0.9055335521697998)
[wall_finder-1] [INFO] [1678735063.467673831] [wall_finder_server]: (0.0, 0.0, 0.4178750813007355, 0.9085044860839844)
[wall_finder-1] [INFO] [1678735063.475781889] [wall_finder_server]: (0.0, 0.0, 0.4149213135242462, 0.9098572731018066)
[wall_finder-1] [INFO] [1678735063.559542112] [wall_finder_server]: (0.0, 0.0, 0.41010400652885437, 0.9120387434959412)
[wall_finder-1] [INFO] [1678735063.575390708] [wall_finder_server]: (0.0, 0.0, 0.40711191296577454, 0.9133782982826233)
[wall_finder-1] [INFO] [1678735063.643771293] [wall_finder_server]: (0.0, 0.0, 0.40220850706100464, 0.9155480861663818)
[wall_finder-1] [INFO] [1678735063.675256264] [wall_finder_server]: (0.0, 0.0, 0.39928385615348816, 0.916827380657196)
[wall_finder-1] [INFO] [1678735063.734875183] [wall_finder_server]: (0.0, 0.0, 0.3945426046848297, 0.9188776612281799)
[wall_finder-1] [INFO] [1678735063.774708355] [wall_finder_server]: (0.0, 0.0, 0.39148056507110596, 0.9201864004135132)
[wall_finder-1] [INFO] [1678735063.810920069] [wall_finder_server]: (0.0, 0.0, 0.3884337544441223, 0.9214766621589661)
[wall_finder-1] [INFO] [1678735063.875599720] [wall_finder_server]: (0.0, 0.0, 0.38481104373931885, 0.922995388507843)
[wall_finder-1] [INFO] [1678735063.877714735] [wall_finder_server]: (0.0, 0.0, 0.3817926347255707, 0.9242480397224426)
[wall_finder-1] [INFO] [1678735063.951955306] [wall_finder_server]: (0.0, 0.0, 0.376997172832489, 0.9262143969535828)
[wall_finder-1] [INFO] [1678735063.977042202] [wall_finder_server]: (0.0, 0.0, 0.3739388883113861, 0.927453339099884)
[wall_finder-1] [INFO] [1678735064.033299544] [wall_finder_server]: (0.0, 0.0, 0.36809012293815613, 0.9297901391983032)
[wall_finder-1] [INFO] [1678735064.076331293] [wall_finder_server]: (0.0, 0.0, 0.36568543314933777, 0.9307385087013245)
[wall_finder-1] [INFO] [1678735064.078327631] [wall_finder_server]: (0.0, 0.0, 0.359956294298172, 0.9329691529273987)
[wall_finder-1] [INFO] [1678735064.152615143] [wall_finder_server]: (0.0, 0.0, 0.3574869930744171, 0.9339181184768677)
[wall_finder-1] [INFO] [1678735064.176854118] [wall_finder_server]: (0.0, 0.0, 0.3521021902561188, 0.9359615445137024)
[wall_finder-1] [INFO] [1678735064.274333926] [wall_finder_server]: (0.0, 0.0, 0.34906473755836487, 0.9370986223220825)
[wall_finder-1] [INFO] [1678735064.276909494] [wall_finder_server]: (0.0, 0.0, 0.34599360823631287, 0.9382368922233582)
[wall_finder-1] [INFO] [1678735064.344679896] [wall_finder_server]: (0.0, 0.0, 0.3411026895046234, 0.940026044845581)
[wall_finder-1] [INFO] [1678735064.376118998] [wall_finder_server]: (0.0, 0.0, 0.3386133313179016, 0.9409255981445312)
[wall_finder-1] [INFO] [1678735064.445204911] [wall_finder_server]: (0.0, 0.0, 0.33131304383277893, 0.9435209035873413)
[wall_finder-1] [INFO] [1678735064.475702848] [wall_finder_server]: (0.0, 0.0, 0.3281845152378082, 0.9446136355400085)
[wall_finder-1] [INFO] [1678735064.551512397] [wall_finder_server]: (0.0, 0.0, 0.32205286622047424, 0.9467216730117798)

As you can see Z and W values are changing even though the robot is still. This error does not happen while working in simulation.

Best Regards,

Ronaldo

2

Hi @w.ronaldo.cd ,

Welcome to the world of sensor reality!
What you notice is called sensor noise or measurement noise. Your fluctuations are not very high.
Even I have noticed this and there is nothing (not exactly) can be done about it to simply get rid of it.
This is where Filters come in handy, like Kalman Filters and Particle Filters along with sensor fusion and related stuff.
The reason why the readings change is because of the physical vibrations that get amplified when not moving. For example, the laser scanner - when the robot is completely still, the laser scanner’s rotation causes some turbulence in the stability of the robot - therefore the IMU is subjected to produce different readings around the same value.

Here is an advice for you:
Since you are working on the wall follower project, odometry values are not of prime importance to you.
Just go around the arena and keep getting the values as you travel.
You can never be exactly at the same point (practically not possible).

So you can have a tolerance value to your odometry readings with some decimation of decimals. Don’t use all the 16 places of decimals. 3 would be more than sufficient.

So, since you do not have an actual problem. I’d say that you are on the right track and you can proceed with your project without digging too deep into this issue for now.

Of course, this will not happen in simulation as things are very much constrained there. Simulation uses simulated noise that lies within certain numerical limits. The real noise has much more dispersion around a central value, therefore simulation cannot exactly replicate real scenario. Although you can get closer to real time situation by tweaking some “noise” parameters in the robot’s URDF file under sensor noise - but you don’t have to worry about this right now. You would know how to do this if you have taken the Kalman Filters course.

Regards,
Girish

1 Like
3

Hi @girishkumar.kannan ,
I don’t think this is just noise. Please check the values printed, you will see that the third value in each line (msg.pose.pose.orientation.z) decrease rapidly. I will shortly post a plot of these values.
Best Regards,
Ronaldo

4

I do think it is noise. How is this preventing your progress?

If you take a look, you will see that the orientation array is a quaternion, not euler. I don’t know about you, but I don’t know how to read quaternions.

In order to see if it’s an actual problem, try the printing same but with euler angles

5

Hi @roalgoal
Below is printed the euler angle with respect to the Z axis (in sexagesimal degrees).

[wall_finder-1] [INFO] [1678734391.610525831] [wall_finder_server]: Angle direccion: "137.4297363436025"
[wall_finder-1] [INFO] [1678734391.613094044] [wall_finder_server]: Angle direccion: "136.88381171626244"
[wall_finder-1] [INFO] [1678734391.696306315] [wall_finder_server]: Angle direccion: "136.50463941110993"
[wall_finder-1] [INFO] [1678734391.711883402] [wall_finder_server]: Angle direccion: "135.9180080234041"
[wall_finder-1] [INFO] [1678734391.777603032] [wall_finder_server]: Angle direccion: "135.54326239438495"
[wall_finder-1] [INFO] [1678734391.811595503] [wall_finder_server]: Angle direccion: "135.02430516039703"
[wall_finder-1] [INFO] [1678734391.870745105] [wall_finder_server]: Angle direccion: "134.64102683072397"
[wall_finder-1] [INFO] [1678734391.912006423] [wall_finder_server]: Angle direccion: "133.95289613226126"
[wall_finder-1] [INFO] [1678734391.952372772] [wall_finder_server]: Angle direccion: "133.57781001410763"
[wall_finder-1] [INFO] [1678734392.010972611] [wall_finder_server]: Angle direccion: "133.01911595864166"
[wall_finder-1] [INFO] [1678734392.049129979] [wall_finder_server]: Angle direccion: "132.64286875570224"
[wall_finder-1] [INFO] [1678734392.109940520] [wall_finder_server]: Angle direccion: "132.05534264903454"
[wall_finder-1] [INFO] [1678734392.111961326] [wall_finder_server]: Angle direccion: "131.68430536241445"
[wall_finder-1] [INFO] [1678734392.193077590] [wall_finder_server]: Angle direccion: "131.15634421436505"
[wall_finder-1] [INFO] [1678734392.209356113] [wall_finder_server]: Angle direccion: "130.77650168153363"
[wall_finder-1] [INFO] [1678734392.280895827] [wall_finder_server]: Angle direccion: "130.08932813473604"
[wall_finder-1] [INFO] [1678734392.309129270] [wall_finder_server]: Angle direccion: "129.7168651909875"
[wall_finder-1] [INFO] [1678734392.387071421] [wall_finder_server]: Angle direccion: "128.95429047354338"
[wall_finder-1] [INFO] [1678734392.409013632] [wall_finder_server]: Angle direccion: "128.58338503462014"
[wall_finder-1] [INFO] [1678734392.471849362] [wall_finder_server]: Angle direccion: "128.0260633677907"
[wall_finder-1] [INFO] [1678734392.508970783] [wall_finder_server]: Angle direccion: "127.64904013988509"
[wall_finder-1] [INFO] [1678734392.511027527] [wall_finder_server]: Angle direccion: "127.10355597040407"
[wall_finder-1] [INFO] [1678734392.582998493] [wall_finder_server]: Angle direccion: "126.71587587104712"
[wall_finder-1] [INFO] [1678734392.608312639] [wall_finder_server]: Angle direccion: "126.14931882272171"
[wall_finder-1] [INFO] [1678734392.689079406] [wall_finder_server]: Angle direccion: "125.76674846861856"
[wall_finder-1] [INFO] [1678734392.707650707] [wall_finder_server]: Angle direccion: "125.02264456762326"
[wall_finder-1] [INFO] [1678734392.775188823] [wall_finder_server]: Angle direccion: "124.64780032356738"
[wall_finder-1] [INFO] [1678734392.807649862] [wall_finder_server]: Angle direccion: "124.06865581434916"
[wall_finder-1] [INFO] [1678734392.869781560] [wall_finder_server]: Angle direccion: "123.69950096260172"
[wall_finder-1] [INFO] [1678734392.907868067] [wall_finder_server]: Angle direccion: "123.03003050874932"
[wall_finder-1] [INFO] [1678734392.960806574] [wall_finder_server]: Angle direccion: "122.65464429392425"
[wall_finder-1] [INFO] [1678734393.007463925] [wall_finder_server]: Angle direccion: "122.12297674668595"
[wall_finder-1] [INFO] [1678734393.049345279] [wall_finder_server]: Angle direccion: "121.74532929868994"
[wall_finder-1] [INFO] [1678734393.106915437] [wall_finder_server]: Angle direccion: "121.14380697181772"
[wall_finder-1] [INFO] [1678734393.110050606] [wall_finder_server]: Angle direccion: "120.76661241672501"
[wall_finder-1] [INFO] [1678734393.191917867] [wall_finder_server]: Angle direccion: "120.18165321359233"
[wall_finder-1] [INFO] [1678734393.206649907] [wall_finder_server]: Angle direccion: "119.79929157847988"
[wall_finder-1] [INFO] [1678734393.284327466] [wall_finder_server]: Angle direccion: "119.21474038896123"
[wall_finder-1] [INFO] [1678734393.306829909] [wall_finder_server]: Angle direccion: "118.84006578546183"
[wall_finder-1] [INFO] [1678734393.372539032] [wall_finder_server]: Angle direccion: "118.19417056700874"
[wall_finder-1] [INFO] [1678734393.407975244] [wall_finder_server]: Angle direccion: "117.81329472713728"
[wall_finder-1] [INFO] [1678734393.410119708] [wall_finder_server]: Angle direccion: "117.2673747638965"
[wall_finder-1] [INFO] [1678734393.485944441] [wall_finder_server]: Angle direccion: "116.89263843991931"
[wall_finder-1] [INFO] [1678734393.506653950] [wall_finder_server]: Angle direccion: "116.29928906298186"
[wall_finder-1] [INFO] [1678734393.574130626] [wall_finder_server]: Angle direccion: "115.91682247249572"
[wall_finder-1] [INFO] [1678734393.605327330] [wall_finder_server]: Angle direccion: "115.34038077407061"
[wall_finder-1] [INFO] [1678734393.661613826] [wall_finder_server]: Angle direccion: "114.96442261028041"
[wall_finder-1] [INFO] [1678734393.705840786] [wall_finder_server]: Angle direccion: "114.44491929147318"
[wall_finder-1] [INFO] [1678734393.749106714] [wall_finder_server]: Angle direccion: "114.06743151919133"
[wall_finder-1] [INFO] [1678734393.805497427] [wall_finder_server]: Angle direccion: "113.4988840710456"
[wall_finder-1] [INFO] [1678734393.842414274] [wall_finder_server]: Angle direccion: "113.12035011638723"
[wall_finder-1] [INFO] [1678734393.904768905] [wall_finder_server]: Angle direccion: "112.50727530299261"
[wall_finder-1] [INFO] [1678734393.906565251] [wall_finder_server]: Angle direccion: "112.13410177061449"
[wall_finder-1] [INFO] [1678734393.991389746] [wall_finder_server]: Angle direccion: "111.50763596713993"
[wall_finder-1] [INFO] [1678734394.004828969] [wall_finder_server]: Angle direccion: "111.12615242852264"
[wall_finder-1] [INFO] [1678734394.078120949] [wall_finder_server]: Angle direccion: "110.4886511602653"
[wall_finder-1] [INFO] [1678734394.106674327] [wall_finder_server]: Angle direccion: "110.11370934188439"
[wall_finder-1] [INFO] [1678734394.108802217] [wall_finder_server]: Angle direccion: "109.56778220540956"
[wall_finder-1] [INFO] [1678734394.192391320] [wall_finder_server]: Angle direccion: "109.193927051914"
[wall_finder-1] [INFO] [1678734394.207305215] [wall_finder_server]: Angle direccion: "108.59327439223952"
[wall_finder-1] [INFO] [1678734394.305888472] [wall_finder_server]: Angle direccion: "108.22056729978156"
[wall_finder-1] [INFO] [1678734394.309240845] [wall_finder_server]: Angle direccion: "107.46665418518545"
[wall_finder-1] [INFO] [1678734394.387379533] [wall_finder_server]: Angle direccion: "107.09082806640139"
[wall_finder-1] [INFO] [1678734394.409444789] [wall_finder_server]: Angle direccion: "106.49247406513064"
[wall_finder-1] [INFO] [1678734394.477795801] [wall_finder_server]: Angle direccion: "106.11881268704616"
[wall_finder-1] [INFO] [1678734394.508842537] [wall_finder_server]: Angle direccion: "105.51370621308997"
[wall_finder-1] [INFO] [1678734394.567924994] [wall_finder_server]: Angle direccion: "105.13230968730078"
[wall_finder-1] [INFO] [1678734394.608291196] [wall_finder_server]: Angle direccion: "104.59540715971143"
[wall_finder-1] [INFO] [1678734394.652656730] [wall_finder_server]: Angle direccion: "104.22282848149956"
[wall_finder-1] [INFO] [1678734394.708231768] [wall_finder_server]: Angle direccion: "103.62606088413996"
[wall_finder-1] [INFO] [1678734394.763022945] [wall_finder_server]: Angle direccion: "103.24581626873483"
[wall_finder-1] [INFO] [1678734394.808584200] [wall_finder_server]: Angle direccion: "102.62049097526763"
[wall_finder-1] [INFO] [1678734394.839539772] [wall_finder_server]: Angle direccion: "102.24560218698694"
[wall_finder-1] [INFO] [1678734394.908174981] [wall_finder_server]: Angle direccion: "101.6740767867787"
[wall_finder-1] [INFO] [1678734394.910002978] [wall_finder_server]: Angle direccion: "101.29243621674603"
[wall_finder-1] [INFO] [1678734394.991735972] [wall_finder_server]: Angle direccion: "100.56537683333603"
[wall_finder-1] [INFO] [1678734395.008458828] [wall_finder_server]: Angle direccion: "100.19471358840372"
[wall_finder-1] [INFO] [1678734395.081026252] [wall_finder_server]: Angle direccion: "99.62119783583054"
[wall_finder-1] [INFO] [1678734395.107757716] [wall_finder_server]: Angle direccion: "99.24813380782494"
[wall_finder-1] [INFO] [1678734395.167776268] [wall_finder_server]: Angle direccion: "98.64978061122389"
[wall_finder-1] [INFO] [1678734395.208075794] [wall_finder_server]: Angle direccion: "98.2719549652103"
[wall_finder-1] [INFO] [1678734395.210134393] [wall_finder_server]: Angle direccion: "97.6421040990596"
[wall_finder-1] [INFO] [1678734395.289461087] [wall_finder_server]: Angle direccion: "97.26945553269968"
[wall_finder-1] [INFO] [1678734395.307361108] [wall_finder_server]: Angle direccion: "96.67965378991815"
[wall_finder-1] [INFO] [1678734395.376333493] [wall_finder_server]: Angle direccion: "96.31147189166455"
[wall_finder-1] [INFO] [1678734395.428145427] [wall_finder_server]: Angle direccion: "95.74173207462874"
[wall_finder-1] [INFO] [1678734395.471473912] [wall_finder_server]: Angle direccion: "95.3629270995422"
[wall_finder-1] [INFO] [1678734395.528142470] [wall_finder_server]: Angle direccion: "94.7396863120122"
[wall_finder-1] [INFO] [1678734395.556573226] [wall_finder_server]: Angle direccion: "94.3684250651447"
[wall_finder-1] [INFO] [1678734395.628059048] [wall_finder_server]: Angle direccion: "93.835340401405"
[wall_finder-1] [INFO] [1678734395.630890630] [wall_finder_server]: Angle direccion: "93.45328741281813"
[wall_finder-1] [INFO] [1678734395.703877609] [wall_finder_server]: Angle direccion: "92.81217563090982"
[wall_finder-1] [INFO] [1678734395.726764100] [wall_finder_server]: Angle direccion: "92.43221203759109"
[wall_finder-1] [INFO] [1678734395.796548094] [wall_finder_server]: Angle direccion: "91.8640888904954"
[wall_finder-1] [INFO] [1678734395.825951435] [wall_finder_server]: Angle direccion: "91.4864496633541"
[wall_finder-1] [INFO] [1678734395.891698643] [wall_finder_server]: Angle direccion: "90.82777758821179"
[wall_finder-1] [INFO] [1678734395.924742020] [wall_finder_server]: Angle direccion: "90.45062438096103"
[wall_finder-1] [INFO] [1678734395.926483747] [wall_finder_server]: Angle direccion: "89.84007353416068"
[wall_finder-1] [INFO] [1678734396.013455448] [wall_finder_server]: Angle direccion: "89.46202588017665"
6

Hi @w.ronaldo.cd ,

Ok, here is another experiment to show this is noise.

Make your robot go straight in a line, then get the readings again and compare the values.

While you are standing still, you will accumulate noise. While you are moving, the noise starts to reduce and gets better (less noisier than before).
When you move in a straight line, the fluctuation would be lesser than when standing still.

If you still have issues, let us know. But just to let you know, odometry values are not very important for the Wall Follower project.

Regards,
Girish

7

Hi @girishkumar.kannan
Thank you for answering my questions and helping me. I used again the real robot and dicovered new things about my problem.

First, to give you more context on what I am working on: I am trying to run just the wall_finder service server node and calling it from command line. I use the /odom topic to get information about the yaw angle of the robot and properly turn it according to the specifications of the project.

Second, I will list some results I got from my latest session using the Real Robot.

  1. When I just started the session and builded my code. I started the wall_finder service server node with a launch file and got the following from the /odom topic (it is important to point out that these results are printed previously to call the server from command line). As you can see here, the values are around 193.5°, the fluctuations are due to sensor noise.
[wall_finder-1] [INFO] [1678845827.430312465] [wall_finder_server]: Angle direccion: "193.5128504053139"
[wall_finder-1] [INFO] [1678845827.432064768] [wall_finder_server]: Angle direccion: "193.51581464844483"
[wall_finder-1] [INFO] [1678845827.526865709] [wall_finder_server]: Angle direccion: "193.52381988611768"
[wall_finder-1] [INFO] [1678845827.531466138] [wall_finder_server]: Angle direccion: "193.52291931144865"
[wall_finder-1] [INFO] [1678845827.621881836] [wall_finder_server]: Angle direccion: "193.52675768467086"
[wall_finder-1] [INFO] [1678845827.634753624] [wall_finder_server]: Angle direccion: "193.5234099497501"
[wall_finder-1] [INFO] [1678845827.706591097] [wall_finder_server]: Angle direccion: "193.52589668288647"
[wall_finder-1] [INFO] [1678845827.741758597] [wall_finder_server]: Angle direccion: "193.51016023879077"
[wall_finder-1] [INFO] [1678845827.743413067] [wall_finder_server]: Angle direccion: "193.51574713494836"
[wall_finder-1] [INFO] [1678845827.842561469] [wall_finder_server]: Angle direccion: "193.5184792285486"
[wall_finder-1] [INFO] [1678845827.846100701] [wall_finder_server]: Angle direccion: "193.51998149004754"
[wall_finder-1] [INFO] [1678845827.847996562] [wall_finder_server]: Angle direccion: "193.5129730676629"
[wall_finder-1] [INFO] [1678845827.935091740] [wall_finder_server]: Angle direccion: "193.5209108390292"
[wall_finder-1] [INFO] [1678845827.955710967] [wall_finder_server]: Angle direccion: "193.51386286016606"
[wall_finder-1] [INFO] [1678845828.027258037] [wall_finder_server]: Angle direccion: "193.51715312296167"
[wall_finder-1] [INFO] [1678845828.065648442] [wall_finder_server]: Angle direccion: "193.5334918282924"
[wall_finder-1] [INFO] [1678845828.099724789] [wall_finder_server]: Angle direccion: "193.52679964259164"
[wall_finder-1] [INFO] [1678845828.167961399] [wall_finder_server]: Angle direccion: "193.51495362782518"
[wall_finder-1] [INFO] [1678845828.169425407] [wall_finder_server]: Angle direccion: "193.51752349564478"
[wall_finder-1] [INFO] [1678845828.218264994] [wall_finder_server]: Angle direccion: "193.51946290371754"
[wall_finder-1] [INFO] [1678845828.290924074] [wall_finder_server]: Angle direccion: "193.50950578870035"
[wall_finder-1] [INFO] [1678845828.292290112] [wall_finder_server]: Angle direccion: "193.51384728163276"
[wall_finder-1] [INFO] [1678845828.377023067] [wall_finder_server]: Angle direccion: "193.5261707693513"
[wall_finder-1] [INFO] [1678845828.392833877] [wall_finder_server]: Angle direccion: "193.52439361506742"
[wall_finder-1] [INFO] [1678845828.394758191] [wall_finder_server]: Angle direccion: "193.5246940806274"
[wall_finder-1] [INFO] [1678845828.507685051] [wall_finder_server]: Angle direccion: "193.52544561462378"
[wall_finder-1] [INFO] [1678845828.509272672] [wall_finder_server]: Angle direccion: "193.52524143030723"
[wall_finder-1] [INFO] [1678845828.555487751] [wall_finder_server]: Angle direccion: "193.52578721312904"
[wall_finder-1] [INFO] [1678845828.610470341] [wall_finder_server]: Angle direccion: "193.52514514954728"
[wall_finder-1] [INFO] [1678845828.611915611] [wall_finder_server]: Angle direccion: "193.50686754052907"
[wall_finder-1] [INFO] [1678845828.680288387] [wall_finder_server]: Angle direccion: "193.51490169282755"
[wall_finder-1] [INFO] [1678845828.712947574] [wall_finder_server]: Angle direccion: "193.5341470693407"
[wall_finder-1] [INFO] [1678845828.775059301] [wall_finder_server]: Angle direccion: "193.52951609447328"
[wall_finder-1] [INFO] [1678845828.814898976] [wall_finder_server]: Angle direccion: "193.5175918332014"
[wall_finder-1] [INFO] [1678845828.874782010] [wall_finder_server]: Angle direccion: "193.5228517991387"
[wall_finder-1] [INFO] [1678845828.916785072] [wall_finder_server]: Angle direccion: "193.52401244797926"
[wall_finder-1] [INFO] [1678845828.918667327] [wall_finder_server]: Angle direccion: "193.52790595954235"
[wall_finder-1] [INFO] [1678845828.965633173] [wall_finder_server]: Angle direccion: "193.52450308510768"
[wall_finder-1] [INFO] [1678845829.020734741] [wall_finder_server]: Angle direccion: "193.51459883215256"
[wall_finder-1] [INFO] [1678845829.022907966] [wall_finder_server]: Angle direccion: "193.5158014586653"
[wall_finder-1] [INFO] [1678845829.090830486] [wall_finder_server]: Angle direccion: "193.5069902044994"
[wall_finder-1] [INFO] [1678845829.122531912] [wall_finder_server]: Angle direccion: "193.51720827089238"
[wall_finder-1] [INFO] [1678845829.155886269] [wall_finder_server]: Angle direccion: "193.52391616705188"
[wall_finder-1] [INFO] [1678845829.252556551] [wall_finder_server]: Angle direccion: "193.5269766237834"
[wall_finder-1] [INFO] [1678845829.254372333] [wall_finder_server]: Angle direccion: "193.52331605900517"
[wall_finder-1] [INFO] [1678845829.271555561] [wall_finder_server]: Angle direccion: "193.52397131428822"
[wall_finder-1] [INFO] [1678845829.356032888] [wall_finder_server]: Angle direccion: "193.53985791392887"
[wall_finder-1] [INFO] [1678845829.358933152] [wall_finder_server]: Angle direccion: "193.53293129840694"
[wall_finder-1] [INFO] [1678845829.406726685] [wall_finder_server]: Angle direccion: "193.52769938545723"
[wall_finder-1] [INFO] [1678845829.462913507] [wall_finder_server]: Angle direccion: "193.5280418067543"
[wall_finder-1] [INFO] [1678845829.464883769] [wall_finder_server]: Angle direccion: "193.5378495098909"
[wall_finder-1] [INFO] [1678845829.525857256] [wall_finder_server]: Angle direccion: "193.53906526866416"
[wall_finder-1] [INFO] [1678845829.564525595] [wall_finder_server]: Angle direccion: "193.53439477459764"
[wall_finder-1] [INFO] [1678845829.624753100] [wall_finder_server]: Angle direccion: "193.53136074128554"
[wall_finder-1] [INFO] [1678845829.683780982] [wall_finder_server]: Angle direccion: "193.5296954659832"
[wall_finder-1] [INFO] [1678845829.685518474] [wall_finder_server]: Angle direccion: "193.54009003583067"
[wall_finder-1] [INFO] [1678845829.720213971] [wall_finder_server]: Angle direccion: "193.5330563472347"
[wall_finder-1] [INFO] [1678845829.785923640] [wall_finder_server]: Angle direccion: "193.5167719494945"
[wall_finder-1] [INFO] [1678845829.787475438] [wall_finder_server]: Angle direccion: "193.52147138195136"
[wall_finder-1] [INFO] [1678845829.834444423] [wall_finder_server]: Angle direccion: "193.5220303586067"
[wall_finder-1] [INFO] [1678845829.888946675] [wall_finder_server]: Angle direccion: "193.53499486834926"
[wall_finder-1] [INFO] [1678845829.890650228] [wall_finder_server]: Angle direccion: "193.53101832226562"
[wall_finder-1] [INFO] [1678845829.977700261] [wall_finder_server]: Angle direccion: "193.52450308510768"
[wall_finder-1] [INFO] [1678845829.991056726] [wall_finder_server]: Angle direccion: "193.5279734710008"
[wall_finder-1] [INFO] [1678845830.050241233] [wall_finder_server]: Angle direccion: "193.51636283147135"
[wall_finder-1] [INFO] [1678845830.092375359] [wall_finder_server]: Angle direccion: "193.5242298221322"
[wall_finder-1] [INFO] [1678845830.117809486] [wall_finder_server]: Angle direccion: "193.52612881141008"

  1. Then, I called the service server from terminal with: ros2 service call /find_wall custom_interfaces/srv/FindWall '{}' . Now, the robot begins to rotate and according to my code it must stop when it is looking toward the nearest wall, but, it keeps rotating (in simulation it works).

  2. At this point I stop the program with control+C. Here something rare happens, the robot keeps rotating even when there is no program running , this is rare because I noticed that the robot always stops inmediately after stopping the programs.

  3. After that, I started again the wall_finder service server node and this time I got what I posted previously about the /odom topic. The direction angle is changing constantly.

[wall_finder-1] [INFO] [1678846767.635615279] [wall_finder_server]: Angle direccion: "24.422501734973697"
[wall_finder-1] [INFO] [1678846767.636357463] [wall_finder_server]: Angle direccion: "24.611719539695375"
[wall_finder-1] [INFO] [1678846767.655419495] [wall_finder_server]: Angle direccion: "24.19459520854638"
[wall_finder-1] [INFO] [1678846767.679583347] [wall_finder_server]: Angle direccion: "24.00558670811429"
[wall_finder-1] [INFO] [1678846767.680006059] [wall_finder_server]: Angle direccion: "23.77203832811299"
[wall_finder-1] [INFO] [1678846767.791278941] [wall_finder_server]: Angle direccion: "23.355676214779127"
[wall_finder-1] [INFO] [1678846767.791826802] [wall_finder_server]: Angle direccion: "23.582807607739376"
[wall_finder-1] [INFO] [1678846767.849335229] [wall_finder_server]: Angle direccion: "23.169828596090444"
[wall_finder-1] [INFO] [1678846767.895505895] [wall_finder_server]: Angle direccion: "22.776793134285928"
[wall_finder-1] [INFO] [1678846767.895900689] [wall_finder_server]: Angle direccion: "22.9836664352733"
[wall_finder-1] [INFO] [1678846767.938474254] [wall_finder_server]: Angle direccion: "22.588282633864264"
[wall_finder-1] [INFO] [1678846768.019590399] [wall_finder_server]: Angle direccion: "22.319016291000885"
[wall_finder-1] [INFO] [1678846768.019948403] [wall_finder_server]: Angle direccion: "22.128958391905766"
[wall_finder-1] [INFO] [1678846768.057445617] [wall_finder_server]: Angle direccion: "21.953470411307176"
[wall_finder-1] [INFO] [1678846768.120934411] [wall_finder_server]: Angle direccion: "21.786258887151686"
[wall_finder-1] [INFO] [1678846768.122152258] [wall_finder_server]: Angle direccion: "21.59542636100849"
[wall_finder-1] [INFO] [1678846768.178431488] [wall_finder_server]: Angle direccion: "21.31709542208997"
[wall_finder-1] [INFO] [1678846768.221887840] [wall_finder_server]: Angle direccion: "21.137439562605888"
[wall_finder-1] [INFO] [1678846768.242339335] [wall_finder_server]: Angle direccion: "20.952827698177817"
[wall_finder-1] [INFO] [1678846768.320210466] [wall_finder_server]: Angle direccion: "20.582220665257182"
[wall_finder-1] [INFO] [1678846768.320803437] [wall_finder_server]: Angle direccion: "20.773699873745944"
[wall_finder-1] [INFO] [1678846768.324084709] [wall_finder_server]: Angle direccion: "20.32135349126628"
[wall_finder-1] [INFO] [1678846768.432925065] [wall_finder_server]: Angle direccion: "20.1347820238876"
[wall_finder-1] [INFO] [1678846768.435475713] [wall_finder_server]: Angle direccion: "19.79792033539139"
[wall_finder-1] [INFO] [1678846768.501421822] [wall_finder_server]: Angle direccion: "19.61141443740501"
[wall_finder-1] [INFO] [1678846768.536512200] [wall_finder_server]: Angle direccion: "19.311809149706495"
[wall_finder-1] [INFO] [1678846768.575958664] [wall_finder_server]: Angle direccion: "19.128850229788167"
[wall_finder-1] [INFO] [1678846768.641495936] [wall_finder_server]: Angle direccion: "18.92344448314451"
[wall_finder-1] [INFO] [1678846768.641700311] [wall_finder_server]: Angle direccion: "18.72882357357822"
[wall_finder-1] [INFO] [1678846768.717493321] [wall_finder_server]: Angle direccion: "18.44641696652292"
[wall_finder-1] [INFO] [1678846768.740322084] [wall_finder_server]: Angle direccion: "18.262467061843427"
[wall_finder-1] [INFO] [1678846768.776720388] [wall_finder_server]: Angle direccion: "18.07609275590697"
[wall_finder-1] [INFO] [1678846768.842054397] [wall_finder_server]: Angle direccion: "17.844234831936472"
[wall_finder-1] [INFO] [1678846768.843624210] [wall_finder_server]: Angle direccion: "17.700950185607898"
[wall_finder-1] [INFO] [1678846768.900323349] [wall_finder_server]: Angle direccion: "17.395246751901905"
[wall_finder-1] [INFO] [1678846768.945197344] [wall_finder_server]: Angle direccion: "17.02044263378855"
[wall_finder-1] [INFO] [1678846768.946778348] [wall_finder_server]: Angle direccion: "17.20483067005167"
[wall_finder-1] [INFO] [1678846769.017263219] [wall_finder_server]: Angle direccion: "16.84888565944092"
[wall_finder-1] [INFO] [1678846769.048279595] [wall_finder_server]: Angle direccion: "16.650488529550373"
[wall_finder-1] [INFO] [1678846769.048488643] [wall_finder_server]: Angle direccion: "16.412357662009153"
[wall_finder-1] [INFO] [1678846769.129542533] [wall_finder_server]: Angle direccion: "16.22728990849828"
[wall_finder-1] [INFO] [1678846769.148409309] [wall_finder_server]: Angle direccion: "16.041907706961034"
[wall_finder-1] [INFO] [1678846769.202666218] [wall_finder_server]: Angle direccion: "15.844727504106968"
[wall_finder-1] [INFO] [1678846769.251915122] [wall_finder_server]: Angle direccion: "15.694209960321574"
[wall_finder-1] [INFO] [1678846769.252703559] [wall_finder_server]: Angle direccion: "15.406696630252009"
[wall_finder-1] [INFO] [1678846769.303864153] [wall_finder_server]: Angle direccion: "15.223678057548609"
[wall_finder-1] [INFO] [1678846769.353975665] [wall_finder_server]: Angle direccion: "14.926116363493966"
[wall_finder-1] [INFO] [1678846769.397506858] [wall_finder_server]: Angle direccion: "14.748197852360088"
[wall_finder-1] [INFO] [1678846769.458977128] [wall_finder_server]: Angle direccion: "14.566816078536602"
[wall_finder-1] [INFO] [1678846769.459801007] [wall_finder_server]: Angle direccion: "14.407980866105165"
[wall_finder-1] [INFO] [1678846769.491492506] [wall_finder_server]: Angle direccion: "14.208494608784168"
[wall_finder-1] [INFO] [1678846769.561102269] [wall_finder_server]: Angle direccion: "13.954295424023279"
[wall_finder-1] [INFO] [1678846769.562106766] [wall_finder_server]: Angle direccion: "13.756572099132024"
[wall_finder-1] [INFO] [1678846769.626206572] [wall_finder_server]: Angle direccion: "13.50217594767578"
[wall_finder-1] [INFO] [1678846769.661333830] [wall_finder_server]: Angle direccion: "13.322422644877442"
[wall_finder-1] [INFO] [1678846769.719848968] [wall_finder_server]: Angle direccion: "12.984048973287544"
[wall_finder-1] [INFO] [1678846769.770638467] [wall_finder_server]: Angle direccion: "12.554992887215162"
[wall_finder-1] [INFO] [1678846769.771091958] [wall_finder_server]: Angle direccion: "12.794467156132843"
[wall_finder-1] [INFO] [1678846769.842931253] [wall_finder_server]: Angle direccion: "12.365015750900819"
[wall_finder-1] [INFO] [1678846769.872967730] [wall_finder_server]: Angle direccion: "11.987028582990492"
[wall_finder-1] [INFO] [1678846769.873351806] [wall_finder_server]: Angle direccion: "12.177295754881383"
[wall_finder-1] [INFO] [1678846769.934573052] [wall_finder_server]: Angle direccion: "11.800243745392466"
[wall_finder-1] [INFO] [1678846769.971802446] [wall_finder_server]: Angle direccion: "11.54197434876394"
[wall_finder-1] [INFO] [1678846770.025561313] [wall_finder_server]: Angle direccion: "11.345924877924674"
[wall_finder-1] [INFO] [1678846770.074261090] [wall_finder_server]: Angle direccion: "11.052533048804063"
[wall_finder-1] [INFO] [1678846770.124806962] [wall_finder_server]: Angle direccion: "10.859413553919065"
[wall_finder-1] [INFO] [1678846770.177954603] [wall_finder_server]: Angle direccion: "10.677276829205265"
[wall_finder-1] [INFO] [1678846770.178371094] [wall_finder_server]: Angle direccion: "10.471050261026356"
[wall_finder-1] [INFO] [1678846770.217429227] [wall_finder_server]: Angle direccion: "10.316598463379059"
[wall_finder-1] [INFO] [1678846770.282045240] [wall_finder_server]: Angle direccion: "10.040812099007146"
[wall_finder-1] [INFO] [1678846770.282264319] [wall_finder_server]: Angle direccion: "9.847738009371412"
[wall_finder-1] [INFO] [1678846770.355182820] [wall_finder_server]: Angle direccion: "9.58696183568976"
[wall_finder-1] [INFO] [1678846770.384336143] [wall_finder_server]: Angle direccion: "9.197643211342154"
[wall_finder-1] [INFO] [1678846770.384711024] [wall_finder_server]: Angle direccion: "9.402806842081489"
[wall_finder-1] [INFO] [1678846770.469465898] [wall_finder_server]: Angle direccion: "9.007834648531237"
[wall_finder-1] [INFO] [1678846770.483445063] [wall_finder_server]: Angle direccion: "8.68107292392924"
[wall_finder-1] [INFO] [1678846770.548027960] [wall_finder_server]: Angle direccion: "8.492816256919525"
[wall_finder-1] [INFO] [1678846770.585695965] [wall_finder_server]: Angle direccion: "8.221502322820285"
[wall_finder-1] [INFO] [1678846770.626756158] [wall_finder_server]: Angle direccion: "8.037648366563474"
[wall_finder-1] [INFO] [1678846770.690300194] [wall_finder_server]: Angle direccion: "7.590354668145753"
[wall_finder-1] [INFO] [1678846770.690740565] [wall_finder_server]: Angle direccion: "7.7654124822202"
[wall_finder-1] [INFO] [1678846770.785543804] [wall_finder_server]: Angle direccion: "7.237814283347357"
[wall_finder-1] [INFO] [1678846770.789974100] [wall_finder_server]: Angle direccion: "7.047156773615434"
[wall_finder-1] [INFO] [1678846770.858365655] [wall_finder_server]: Angle direccion: "6.828727470659963"
[wall_finder-1] [INFO] [1678846770.893921699] [wall_finder_server]: Angle direccion: "6.411785499479933"
[wall_finder-1] [INFO] [1678846770.894940059] [wall_finder_server]: Angle direccion: "6.637202164391583"
[wall_finder-1] [INFO] [1678846770.977103414] [wall_finder_server]: Angle direccion: "6.217461481459017"
[wall_finder-1] [INFO] [1678846770.994152093] [wall_finder_server]: Angle direccion: "6.026530394339776"
[wall_finder-1] [INFO] [1678846771.050192093] [wall_finder_server]: Angle direccion: "5.843831097839254"
[wall_finder-1] [INFO] [1678846771.098583066] [wall_finder_server]: Angle direccion: "5.653147628891123"
[wall_finder-1] [INFO] [1678846771.099125844] [wall_finder_server]: Angle direccion: "5.411137143592086"
[wall_finder-1] [INFO] [1678846771.179537991] [wall_finder_server]: Angle direccion: "5.221536425740145"
[wall_finder-1] [INFO] [1678846771.197834670] [wall_finder_server]: Angle direccion: "4.950375778215389"
[wall_finder-1] [INFO] [1678846771.257087705] [wall_finder_server]: Angle direccion: "4.769051488273101"
[wall_finder-1] [INFO] [1678846771.300450377] [wall_finder_server]: Angle direccion: "4.511200137675787"
[wall_finder-1] [INFO] [1678846771.302375270] [wall_finder_server]: Angle direccion: "4.326069415709371"
[wall_finder-1] [INFO] [1678846771.359770852] [wall_finder_server]: Angle direccion: "4.11989328255834"
[wall_finder-1] [INFO] [1678846771.405655951] [wall_finder_server]: Angle direccion: "3.6345231433981224"
[wall_finder-1] [INFO] [1678846771.406657817] [wall_finder_server]: Angle direccion: "3.9351860057834216"
[wall_finder-1] [INFO] [1678846771.479247323] [wall_finder_server]: Angle direccion: "3.4428984744205335"
[wall_finder-1] [INFO] [1678846771.504648407] [wall_finder_server]: Angle direccion: "3.1905384235744765"
[wall_finder-1] [INFO] [1678846771.573805854] [wall_finder_server]: Angle direccion: "3.0037210031302677"
[wall_finder-1] [INFO] [1678846771.621592156] [wall_finder_server]: Angle direccion: "2.819958069008083"
[wall_finder-1] [INFO] [1678846771.623377293] [wall_finder_server]: Angle direccion: "2.6566856876848415"
[wall_finder-1] [INFO] [1678846771.707261775] [wall_finder_server]: Angle direccion: "2.4432475621433594"
[wall_finder-1] [INFO] [1678846771.723107444] [wall_finder_server]: Angle direccion: "2.1809135725850086"
[wall_finder-1] [INFO] [1678846771.765925311] [wall_finder_server]: Angle direccion: "1.9884149903676895"
[wall_finder-1] [INFO] [1678846771.846261634] [wall_finder_server]: Angle direccion: "1.4559965790975236"
[wall_finder-1] [INFO] [1678846771.846633410] [wall_finder_server]: Angle direccion: "1.6498048563272556"
[wall_finder-1] [INFO] [1678846771.883548685] [wall_finder_server]: Angle direccion: "1.271685563457312"
[wall_finder-1] [INFO] [1678846771.948539250] [wall_finder_server]: Angle direccion: "1.109934728326356"
[wall_finder-1] [INFO] [1678846771.949000007] [wall_finder_server]: Angle direccion: "0.9038935527375956"
[wall_finder-1] [INFO] [1678846772.003298075] [wall_finder_server]: Angle direccion: "0.6244619976872754"
[wall_finder-1] [INFO] [1678846772.066732287] [wall_finder_server]: Angle direccion: "0.4444233320363667"
[wall_finder-1] [INFO] [1678846772.067200005] [wall_finder_server]: Angle direccion: "0.2126852613141465"
[wall_finder-1] [INFO] [1678846772.140256403] [wall_finder_server]: Angle direccion: "0.026434540573060168"
[wall_finder-1] [INFO] [1678846772.168695865] [wall_finder_server]: Angle direccion: "359.62918048882483"
[wall_finder-1] [INFO] [1678846772.169196921] [wall_finder_server]: Angle direccion: "359.81371000158094"
[wall_finder-1] [INFO] [1678846772.242530866] [wall_finder_server]: Angle direccion: "359.3506453567803"
[wall_finder-1] [INFO] [1678846772.287342072] [wall_finder_server]: Angle direccion: "359.1707177069827"
[wall_finder-1] [INFO] [1678846772.287785730] [wall_finder_server]: Angle direccion: "358.92594076519475"
[wall_finder-1] [INFO] [1678846772.382075062] [wall_finder_server]: Angle direccion: "358.7424818504949"
[wall_finder-1] [INFO] [1678846772.386954620] [wall_finder_server]: Angle direccion: "358.4733143942414"
[wall_finder-1] [INFO] [1678846773.268515771] [wall_finder_server]: Angle direccion: "353.80881440890937"
[wall_finder-1] [INFO] [1678846773.305770805] [wall_finder_server]: Angle direccion: "353.6225003431028"
[wall_finder-1] [INFO] [1678846773.334978393] [wall_finder_server]: Angle direccion: "353.4737320005946"
[wall_finder-1] [INFO] [1678846773.402907438] [wall_finder_server]: Angle direccion: "353.04963887212296"
[wall_finder-1] [INFO] [1678846773.403736746] [wall_finder_server]: Angle direccion: "353.22801592158424"
[wall_finder-1] [INFO] [1678846773.408229817] [wall_finder_server]: Angle direccion: "352.8125152305411"
[wall_finder-1] [INFO] [1678846773.497111652] [wall_finder_server]: Angle direccion: "352.63022032861255"
[wall_finder-1] [INFO] [1678846773.510432809] [wall_finder_server]: Angle direccion: "352.44584320607373"
[wall_finder-1] [INFO] [1678846773.566872949] [wall_finder_server]: Angle direccion: "352.27290958146165"
[wall_finder-1] [INFO] [1678846773.613562377] [wall_finder_server]: Angle direccion: "352.11043647295236"
[wall_finder-1] [INFO] [1678846773.614243447] [wall_finder_server]: Angle direccion: "351.7882568022953"
[wall_finder-1] [INFO] [1678846773.669400306] [wall_finder_server]: Angle direccion: "351.61447586749136"
[wall_finder-1] [INFO] [1678846773.718272367] [wall_finder_server]: Angle direccion: "351.3954932985817"
[wall_finder-1] [INFO] [1678846773.762498230] [wall_finder_server]: Angle direccion: "351.20024208696327"

I tried to return to the condition of the point 1 rebuilding and reconnecting to the robot, but it did not happen.

All my code for the find_wall service server is posted bellow. My code works perfectly in simulation, but with the real robot I am having these problems.

# import the Twist module from geometry_msgs messages interface
from geometry_msgs.msg import Twist
# import the MyCustomServiceMessage module from custom_interfaces_service interface
from custom_interfaces.srv import FindWall
# import the ROS2 Python client libraries
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from rclpy.qos import ReliabilityPolicy, QoSProfile

import numpy as np
from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor
from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
import time

class WallFinder(Node):

    def __init__(self):
        # Here you have the class constructor
        self.group1 = MutuallyExclusiveCallbackGroup()
        self.group2 = MutuallyExclusiveCallbackGroup()
        self.group3 = MutuallyExclusiveCallbackGroup()
        # call the class constructor to initialize the node as service_stop
        super().__init__('wall_finder_server')

        self.odom_subscriber_ = self.create_subscription(
            Odometry,
            '/odom',
            self.odom_callback,
            QoSProfile(depth=10, reliability=ReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE),
            callback_group=self.group2)  

        self.laser_subscriber_ = self.create_subscription(
            LaserScan,
            '/scan',
            self.laser_callback,
            QoSProfile(depth=10, reliability=ReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE),
            callback_group=self.group1)  

        self.wall_finder_srv = self.create_service(
            FindWall, 
            'find_wall', 
            self.wall_finder_srv_callback,
            callback_group=self.group3)

        self.cmd_vel_publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
        self.nearest_wall_identified = False
        self.angle_fisrt_turn = 0.0
        self.angular_z = 0.1
        self.linear_x = 0.08
        self.angle_to_rotate = 0.0
        self.robot_yaw = 0.0
        self.angle_tol = 10 * np.pi / 180
        self.dist_tol = 0.1

        self.print_timer_period = 3
        # self.print_timer = self.create_timer(self.print_timer_period, self.print_function)

        self.first_turn_finished = False
        self.dist_nearest_wall = 0.0
        self.dist_to_wall = 0.0

    def laser_callback(self, msg):
        # print the log info in the terminal
        # self.get_logger().info(str(self.nearest_wall_identified))
        if not self.nearest_wall_identified:
            ranges_length = round((2 * np.pi / msg.angle_increment) + 1)
            ranges = msg.ranges[:]
            # for i in range(ranges_length):
            #     ranges.append(msg.ranges[i])
            ranges = np.array(ranges)
            min_range_idx = np.argmin(ranges)
            self.get_logger().info('Min range idx: "%s"' % str(min_range_idx))

            min_range_angle = (2 * np.pi * (min_range_idx + 1)) / ranges_length
            angle_to_rotate = min_range_angle - np.pi
            self.angle_to_rotate = angle_to_rotate
            self.nearest_wall_identified = True
            self.dist_nearest_wall = msg.ranges[min_range_idx]
        
        self.dist_to_wall = msg.ranges[360]
    
    def print_function(self):
        self.get_logger().info('Robot yaw angle: "%s"' % str(self.robot_yaw * 180/ np.pi))
        self.get_logger().info('Dist to wall: "%s"' % str(self.dist_to_wall))

    def wall_finder_srv_callback(self, request, response):
        msg = Twist()
        robot_initial_yaw = self.robot_yaw

        rotation_z = (robot_initial_yaw + self.angle_to_rotate)%(2*np.pi)
        quadrant_idx = round(rotation_z / (np.pi/2))
        objective_angle = (quadrant_idx * np.pi / 2)%(2*np.pi)

        self.get_logger().info('Initial yaw angle: "%s"' % str(robot_initial_yaw * 180/ np.pi))
        self.get_logger().info('Objective angle: "%s"' % str(objective_angle * 180/ np.pi))
        
        while not self.nearest_wall_identified:
            self.get_logger().info('WAITING FOR IDENTIFYING THE NEAREST WALL')
            time.sleep(0.5)
            
        if self.nearest_wall_identified is True:
            while abs(self.robot_yaw - objective_angle)%(2*np.pi) > self.angle_tol:
                if self.angle_to_rotate > 0:
                    msg.angular.z = self.angular_z
                elif self.angle_to_rotate < 0:
                    msg.angular.z = -self.angular_z
                self.cmd_vel_publisher_.publish(msg)
            msg.angular.z = 0.0
            self.cmd_vel_publisher_.publish(msg)
            self.first_turn_finished = True
        
        linear_x_sign = 1
        if self.dist_to_wall < 0.3:
            linear_x_sign = -1
        while abs(self.dist_to_wall - 0.3) > self.dist_tol:
            msg.linear.x = linear_x_sign * self.linear_x
            msg.angular.z = 0.0
            self.cmd_vel_publisher_.publish(msg)
        msg.linear.x = 0.0
        self.cmd_vel_publisher_.publish(msg)

        final_objective_angle = (objective_angle + (np.pi / 2.0)) % (2*np.pi)
        self.get_logger().info('Last objective angle: "%s"' % str(final_objective_angle * 180/ np.pi))
        while abs(self.robot_yaw - final_objective_angle) % (2 * np.pi) > self.angle_tol:
            msg.linear.x = 0.0
            msg.angular.z = self.angular_z
            self.cmd_vel_publisher_.publish(msg)

        msg.linear.x = 0.0
        msg.angular.z = 0.0
        self.cmd_vel_publisher_.publish(msg)
        
        response.wallfound = True
        
        return response
  
    def odom_callback(self, msg):
        # print the log info in the terminal
        x = msg.pose.pose.orientation.x
        y = msg.pose.pose.orientation.y
        z = msg.pose.pose.orientation.z
        w = msg.pose.pose.orientation.w

        _, _, yaw = self.euler_from_quaternion([x, y, z, w])
        # self.get_logger().info('Yaw angle: "%s"' % str(yaw * 180/ np.pi))

        rotation_z = yaw % np.pi
        if yaw < 0:
            rotation_z += np.pi
        
        self.robot_yaw = rotation_z
        self.get_logger().info('Angle direccion: "%s"' % str(rotation_z * 180/ np.pi))
    
    def euler_from_quaternion(self, quaternion):
        """
        Converts quaternion (w in last place) to euler roll, pitch, yaw
        quaternion = [x, y, z, w]
        Below should be replaced when porting for ROS2 Python tf_conversions is done.
        """
        x = quaternion[0]
        y = quaternion[1]
        z = quaternion[2]
        w = quaternion[3]

        sinr_cosp = 2 * (w * x + y * z)
        cosr_cosp = 1 - 2 * (x * x + y * y)
        roll = np.arctan2(sinr_cosp, cosr_cosp)

        sinp = 2 * (w * y - z * x)
        pitch = np.arcsin(sinp)

        siny_cosp = 2 * (w * z + x * y)
        cosy_cosp = 1 - 2 * (y * y + z * z)
        yaw = np.arctan2(siny_cosp, cosy_cosp)

        return roll, pitch, yaw


def main(args=None):
    # initialize the ROS communication
    rclpy.init(args=args)
    wall_finder_node = WallFinder()
    # Create a MultiThreadedExecutor with 3 threads
    executor = MultiThreadedExecutor(num_threads=4)
    # Add the node to the executor
    executor.add_node(wall_finder_node)
    try:
        # Spin the executor
        executor.spin()
    finally:
        # Shutdown the executor
        executor.shutdown()
        wall_finder_node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

Best Regards,

Ronaldo

8

Hi @w.ronaldo.cd ,

What I can’t understand is, how are you able to use odometry “yaw” for finding the wall? It would make sense to me if you say you are using laser scan’s values to find which is the closest direction of the wall. But odometry (without the info from laser scanner) is useless in finding a wall - unless you are implementing a logic to see if the robot is heading towards 0 / 90 / 180 / 270 degrees - and technically this logic will not be very helpful.

Yes, the values do not seem to fluctuate very much. It is quite consistent.

I believe there is a small communication lag in the robot that causes the robot to overshoot the stop position. The best way to handle this is to implement a “Stop-Measure-Rotate” cycle until you find the correct angle to stop the rotation.
To explain further on this point:
The time taken to interpret the laser scan values is higher than the time taken to make a rotation on your robot. Hence your robot still does not know whether to stop or continue rotating when it has reached the correct angle to stop. Since the calculation time takes long, the robot has already rotated past the stop point, hence your robot continues to spin. If you use a “Measure-while-Rotating” logic, then your robot will continue to rotate endlessly. You must use “Stop-Measure-Rotate” method.

Yes, that may be true with simulation, but not in case of real robot. Have you implemented a ShutdownHook in your code? The shutdown hook is has the code responsible for stopping and terminating all programs safely on the robot. This code has the logic to handle exceptions and stop the robot.

I think at this point, your robot is having a very small rotational velocity. Your robot has not come to a complete stop. It seems to be rotating at around 8 degrees / second (roughly).
So either the robot is drifting or the sensor is drifting after the robot movement.

I see a HUGE problem here! When you are rotating, you want to and you SHOULD have the Odometry and Laser Scanner values read simultaneously, that is in parallel, working together.

You have defined MutExCbG for your Service, Laser and Odom callbacks. Naturally, your code will not work properly in the real robot. You must have Laser and Odom callbacks in the ReentrantCallbackGroup, because you need them to be running together.
Now, naturally, you also want Laser Scan and Odometry running along when your Service Callback runs. Therefore, you must set all your callbacks as ReentCbG.
MutuallyExclusiveCallbackGroup is a series execution model - executes one after other.
ReentrantCallbackGroup is a parallel execution model - executes together, in parallel.

Summary of Key Points:

  1. Do not measure while rotating. Stop, measure and then rotate.
  2. Make sure when you stop the robot, the robot comes to complete stop.
  3. Use ReentCbG instead of MutExCbG.
  4. Using odometry (yaw) alone to find wall is a bad idea.

I hope this helps solves your issue.

Regards,
Girish

1 Like
9

This topic was automatically closed 5 days after the last reply. New replies are no longer allowed.