Move_basic tutorial / is my map ok?


#1

Hello,

Compared to lidar slam, I am struggling to understand whether or not my fiducial map is ok.

the fiducials do seem to be recognized, but maybe not at the proper location.
Assuming the grid in rviz is “regular ground”. i see in the grid options that it seems to be the XY plane 10m x10m. but then I have fiducials whose Z location cannot be correct

  • some fiducials are below the ground => impossible
  • i measured my lab ceiling at 2.85 meters from the ground floor, while some seems to be faaaaar away from the ground (say > 100m) => impossible too

for the record I taped fiducials directly on the ceiling (those are horizontal then), and others are positioned vertically on beams (dont know if that can be a problem either, i thought orientation of the fiducal was not important so long that you can see it on the video). I also have fiducials that are taped below tables (i noticed that the magni would often see thoses surfaces)

assuming the generated map (/home/ubuntu/.ros/slam/map.txt) has each line starting with the fiducial id and then a bunch of parameters, then for instance the last line (id 827) does not make sense because i printed fiducials from 100 to 199 (and taped up to 185)

also the move_basic tutorial is simply too short for me to get it to work :’(

By looking at this thread i realized i had to start (ssh on robot)
roslaunch magni_nav move_basic.launch
while
roslaunch magni_nav aruco.launch
was already running

and then i selected a simple 2D goal in rviz and the magni starts rotating on itself and starts moving kinda straight ahead for some time (till a wall would basically stop it). I struggle to read the map therefore I dont even know where the robot is nor where i sent it.
(btw the way, it would help to have a RobotModel working in rviz. it would give a better sense of scale and location on the map)

can we have a move_basic tutorial i can work with?

ubuntu@ubiquityrobot:~$ cat /home/ubuntu/.ros/slam/map.txt
101 -26.656808 99.165719 162.816975 -72.716180 -0.457794 138.139923 100.000000 1512 107 109 111 112 115 116 120 121 126 132 140 141 144 145 146 147 148 149 150 151 153 155 157 158 159 167 169 170 171 181 183 184
103 -0.572407 1.534580 1.050110 102.866532 60.377331 108.790836 3.345388 526 107 148 152 154 155 156 158 159 160 163 164 165 166 167 168 169 170 171 172 174 175 179 182 184
104 16.984606 16.845135 7.230391 -120.654368 -15.142412 158.825007 100.000000 160 111 116 122 123 124 125 126 127 128 129 133 136 137 138 139 142 143 145 146 148 181 827
106 11.039275 0.770654 10.798956 -171.234260 -41.369556 106.219993 100.000000 50 107 109 112 115 119 130 131 132 133 134 135 141 159 162 183 184
107 -14.848998 63.490566 60.331391 -136.872514 57.310953 -127.775982 100.000000 2214 101 103 106 109 112 115 117 120 121 126 130 131 132 140 141 144 155 156 157 159 160 162 163 169 171 173 183 184
109 -14.034703 63.880796 62.170842 172.170302 -46.052949 33.145441 100.000000 298 101 106 107 112 115 116 118 119 120 121 122 126 130 131 132 133 134 140 141 144 156 157 159 160 162 169 171 183 184
111 1.611886 2.781166 0.622700 -154.118435 6.667545 -82.040257 100.000000 54 101 104 121 124 125 126 127 128 136 138 142 143 145 146 157 159 171 181 183
112 -66.870347 159.681575 272.682916 111.844052 -63.030944 -71.783185 100.000000 3842 101 106 107 109 115 116 117 119 120 130 131 132 133 134 135 140 141 144 159 162 169 181 183 184
115 3.222911 1.053639 3.538570 129.805258 0.780920 1.405870 0.013341 206 101 106 107 109 112 116 118 119 120 121 122 130 131 132 133 134 135 141 156 157 159 160 162 169 171 181 183 184
116 -65.261925 159.541183 269.611051 146.551928 -2.272591 -82.634097 100.000000 678 101 104 109 112 115 117 118 120 122 123 124 125 126 127 128 129 132 136 137 138 139 140 141 142 143 144 145 159 169 183 184 827
117 5.285672 2.786595 2.296180 174.463559 -47.953249 -83.155212 0.001000 300 107 112 116 118 119 120 121 129 131 132 133 135 136 137 138 139 140 141 162
118 20.747389 14.844058 9.334804 155.305799 -58.863819 -87.715952 100.000000 44 109 115 116 117 119 120 122 129 131 132 133 134 135 136 137 138 139 140 141 162
119 4.158476 2.019331 3.278833 -143.814336 36.176459 99.264548 0.001000 118 106 109 112 115 117 118 120 121 122 130 131 132 133 134 135 140 141 159 162 181 183
120 -66.712222 158.562178 272.762757 115.782257 41.027085 -43.701676 100.000000 7108 101 107 109 112 115 116 117 118 119 121 122 131 132 133 135 140 141 144 159 160 162 169 181 183 184
121 -6.667172 42.925226 38.786459 147.840508 -34.472897 35.197277 100.000000 82 101 107 109 111 115 117 119 120 124 126 136 138 140 145 159 160 162 169 181 183 184
122 15.124032 13.958397 9.229935 -162.648079 58.614062 82.758762 100.000000 10 104 109 115 116 118 119 120 123 124 125 126 131 133 136 137 139 162
123 17.897307 16.526710 7.611123 -149.575803 55.626817 94.288510 100.000000 34 104 116 122 124 125 126 127 133 136 137 138 139 159
124 16.805097 17.157477 6.655900 -120.219753 -15.160915 159.727446 100.000000 226 104 111 116 121 122 123 125 126 127 128 129 133 136 137 138 139 142 143 145 146 148 181 827
125 16.985522 17.670627 6.000431 -119.904074 -11.998233 155.798554 100.000000 262 104 111 116 122 123 124 126 127 128 129 133 136 137 138 139 142 143 145 146 147 148 149 151 153 827
126 5.137574 4.665451 -1.518516 36.308889 1.633414 4.956208 100.000000 484 101 104 107 109 111 116 121 122 123 124 125 127 128 129 133 136 137 138 139 142 143 145 146 147 148 149 159 183 827
127 17.987817 17.516310 5.561792 32.150471 14.850813 -21.097730 100.000000 394 104 111 116 123 124 125 126 128 129 136 137 138 139 142 143 145 146 148 827
128 20.397381 25.258548 8.581904 130.725881 12.435042 -23.236318 100.000000 310 104 111 116 124 125 126 127 129 136 137 138 139 142 143 145 146 148 827
129 21.113225 16.907359 6.703789 47.015806 9.338405 -39.946195 100.000000 382 104 116 117 118 124 125 126 127 128 132 136 137 138 139 140 141 142 143 145 146 827
130 4.845714 0.695299 4.220978 138.953221 -2.748091 179.393788 100.000000 50 106 107 109 112 115 119 131 133 134 135 141 156 160 162 169 171 183 184
131 11.267044 1.539463 10.162910 -82.987272 44.183927 113.610677 100.000000 18 106 107 109 112 115 117 118 119 120 122 130 132 133 134 135 140 141 162 183
132 -65.715114 159.338616 275.726532 145.371559 -16.110922 -78.666269 100.000000 3040 101 106 107 109 112 115 116 117 118 119 120 129 131 133 134 135 136 139 140 141 144 159 160 162 169 183 184
133 15.645478 11.869560 9.675060 154.679725 -54.723025 -86.493518 100.000000 38 104 106 109 112 115 117 118 119 120 122 123 124 125 126 130 131 132 134 135 136 137 139 140 141 159 162 183 184
134 12.176464 -0.027999 10.227693 -116.274436 -63.506105 97.504907 100.000000 36 106 109 112 115 118 119 130 131 132 133 135 141 162 183
135 9.633984 1.374510 8.412357 -129.855433 -25.306358 157.741710 100.000000 40 106 112 115 117 118 119 120 130 131 132 133 134 140 141 159 162 183 184
136 19.046523 17.053815 6.096283 121.022877 12.825371 -20.981925 100.000000 360 104 111 116 117 118 121 122 123 124 125 126 127 128 129 132 133 137 138 139 140 141 142 143 145 146 827
137 20.086438 16.479119 5.691924 71.905400 -55.090342 -90.990699 100.000000 270 104 116 117 118 122 123 124 125 126 127 128 129 133 136 138 139 140 141 142 143 145 146
138 19.340523 16.562859 6.531751 157.280204 -56.052746 -91.019043 100.000000 294 104 111 116 117 118 121 123 124 125 126 127 128 129 136 137 139 140 141 142 143 145 146 827
139 20.003505 15.019741 6.486823 108.516767 -54.508877 -111.697146 100.000000 220 104 116 117 118 122 123 124 125 126 127 128 129 132 133 136 137 138 140 141 142 143 145
140 -65.484628 161.301736 275.424292 159.441116 -12.893577 -78.075237 100.000000 3870 101 107 109 112 116 117 118 119 120 121 129 131 132 133 135 136 137 138 139 141 144 159 162 183 184
141 -65.109913 158.506491 271.960012 155.086193 -36.596081 -68.305932 100.000000 1022 101 106 107 109 112 115 116 117 118 119 120 129 130 131 132 133 134 135 136 137 138 139 140 144 159 162 169 183 184
142 -1.633465 7.770602 -0.899068 109.725577 43.966725 152.949581 100.000000 236 104 111 116 124 125 126 127 128 129 136 137 138 139 143 145 146 147 148 149 150 151 152 153 181 183 827
143 -1.966882 6.063215 -0.511878 139.248615 -3.987864 27.848866 100.000000 156 104 111 116 124 125 126 127 128 129 136 137 138 139 142 145 146 147 148 149 150 151 152 153 159 168 181 182 183 827
144 -66.744127 161.971101 271.369618 80.311331 -60.399993 48.046483 100.000000 1138 101 107 109 112 116 120 132 140 141 159 183 184
145 -1.347396 4.949896 0.055302 173.027044 61.777147 107.931005 100.000000 144 101 104 111 116 121 124 125 126 127 128 129 136 137 138 139 142 143 146 147 148 149 150 151 153 157 159 171 181 182 183 827
146 0.661006 4.288554 -1.545255 -1.437569 -5.071684 -19.977574 0.001000 194 101 104 111 124 125 126 127 128 129 136 137 138 142 143 145 147 148 149 150 151 152 153 159 163 167 168 170 181 182 183
147 0.205691 4.273929 -2.303195 24.649068 3.428368 2.896541 0.019077 124 101 125 126 142 143 145 146 148 149 150 151 152 153 158 163 167 168 170 182 183
148 0.000548 4.140709 -1.020905 24.343857 10.105785 -1.642057 0.001000 160 101 103 104 124 125 126 127 128 142 143 145 146 147 149 150 151 152 153 155 158 163 167 168 170 175 181 182 183 184
149 -3.427857 4.344001 -1.055203 86.866834 63.048013 112.175667 100.000000 156 101 125 126 142 143 145 146 147 148 150 151 152 153 155 158 167 170 177 181 182 183
150 -0.721035 4.005769 -1.970389 111.098774 1.935095 1.416693 0.001000 148 101 142 143 145 146 147 148 149 151 152 153 155 158 163 167 168 170 177 182 183
151 -0.610879 3.649788 -1.111311 -110.834804 -1.956421 -178.091389 0.001000 202 101 125 142 143 145 146 147 148 149 150 152 153 155 158 159 163 167 168 170 175 177 182
152 -0.500751 4.319641 -4.404629 158.902295 -39.877355 -105.974394 68.637616 102 103 142 143 146 147 148 149 150 151 153 158 163 168 170 175 182
153 -2.959097 4.045860 0.754333 -109.505743 -72.574590 -101.565993 100.000000 104 101 125 142 143 145 146 147 148 149 150 151 152 155 158 167 170 183
154 -0.272738 1.757837 2.740733 66.534582 2.554880 2.338564 100.000000 26 103 155 156 158 159 160 163 165 169 171 172 174 179 184
155 -0.191479 1.332667 2.262354 -118.248149 -4.949845 -177.250434 0.020859 514 101 103 107 148 149 150 151 153 154 156 158 159 160 162 163 165 167 169 170 171 172 174 177 179 184
156 -0.117911 0.844990 3.134216 -116.851060 -5.408613 -176.659277 0.000000 0 103 107 109 115 130 154 155 158 159 160 162 163 164 169 171 172 173 174 175 176 177 179 184
157 2.839016 4.151251 2.273034 58.762916 -9.794590 -74.411976 100.000000 32 101 107 109 111 115 145 159 162 171 181 183 184
158 1.769677 1.249680 3.400524 58.191913 -59.622755 2.580862 100.000000 42 101 103 147 148 149 150 151 152 153 154 155 156 159 163 167 170 171 172 174 179
159 -67.950868 157.689725 270.946513 117.997764 -28.313238 79.799696 100.000000 7294 101 103 106 107 109 111 112 115 116 119 120 121 123 126 132 133 135 140 141 143 144 145 146 151 154 155 156 157 158 160 162 163 165 167 169 170 171 172 174 179 181 183 184
160 0.820510 0.372217 3.867086 141.225431 4.906022 -163.624835 0.008570 650 103 107 109 115 120 121 130 132 154 155 156 159 162 163 165 169 171 172 173 176 177 179 183 184
162 2.843596 -0.196771 3.903112 138.962442 -0.824640 179.369715 0.001000 594 106 107 109 112 115 117 118 119 120 121 122 130 131 132 133 134 135 140 141 155 156 157 159 160 169 171 181 183 184
163 -0.592335 0.850904 0.142900 97.249475 -26.907223 95.015082 1.388748 622 103 107 146 147 148 150 151 152 154 155 156 158 159 160 164 165 166 167 168 169 170 171 172 174 175 176 177 179 182
164 -2.717046 1.158147 3.475596 121.237696 2.285332 1.797175 0.001000 66 103 156 163 165 166 168 170 173 175 176 177 179
165 -2.324906 1.855634 2.741900 128.291706 -1.157627 12.701030 0.008946 42 103 154 155 159 160 163 164 166 168 169 170 172 173 174 176 177
166 -3.325618 1.067994 2.951698 106.115476 52.730447 101.479282 0.001000 94 103 163 164 165 173 176 177
167 1.188292 4.256186 -0.480116 -35.113351 45.871238 171.349230 100.000000 62 101 103 146 147 148 149 150 151 153 155 158 159 163 170 172 174 182
168 -3.205749 4.012707 -2.223045 104.208426 43.224535 66.038846 0.453474 46 103 143 146 147 148 150 151 152 163 164 165 170 172 174 175 182
169 1.326990 -0.155565 4.058681 164.570626 -61.339591 -137.226864 1.511115 638 101 103 107 109 112 115 116 120 121 130 132 141 154 155 156 159 160 162 163 165 171 172 173 176 177 179 183 184
170 -3.629946 2.769036 0.367576 107.277632 57.946702 103.932534 0.022280 92 101 103 146 147 148 149 150 151 152 153 155 158 159 163 164 165 167 168 172 174 175 176 177 179 182
171 0.543587 1.115883 3.820891 -120.561688 1.667807 178.925381 0.001000 538 101 103 107 109 111 115 130 145 154 155 156 157 158 159 160 162 163 169 172 173 174 177 179 181 183 184
172 -2.684022 1.968710 1.599379 117.295251 5.932980 -0.123343 0.001000 376 103 154 155 156 158 159 160 163 165 167 168 169 170 171 174 179 182
173 -1.386807 0.368773 4.659579 149.893068 -0.515490 -172.810280 0.001000 484 107 156 160 164 165 166 169 171 175 176 177 179
174 -3.268376 1.604001 1.830245 99.387260 60.809134 100.023584 0.003695 92 103 154 155 156 158 159 163 165 167 168 170 171 172 179 182
175 -1.605736 2.253857 1.322621 159.675380 -59.155231 -70.126747 0.001000 50 103 148 151 152 156 163 164 168 170 173 176 177 179 182
176 -3.192780 0.533830 3.887325 93.477249 59.493239 98.383156 0.001000 142 156 160 163 164 165 166 169 170 173 175 177 179
177 -1.917089 1.048249 4.037426 176.693548 54.124703 101.484534 0.001000 98 149 150 151 155 156 160 163 164 165 166 169 170 171 173 175 176 179
178 2.546964 0.747310 1.831427 168.965157 -49.629209 -70.198512 0.001000 14 181
179 -1.097206 1.447612 2.897297 117.239720 3.848009 -1.874027 0.001000 62 103 154 155 156 158 159 160 163 164 169 170 171 172 173 174 175 176 177 184
181 2.529600 1.179332 1.242516 -129.627305 1.481775 -177.005601 0.001000 86 101 104 111 112 115 119 120 121 124 142 143 145 146 148 149 157 159 162 171 178 183 184
182 -0.739008 3.670973 -2.346663 19.551067 1.627287 1.344502 0.001000 116 103 143 145 146 147 148 149 150 151 152 163 167 168 170 172 174 175
183 -67.802839 158.776726 271.451146 -145.905903 64.607533 -135.611667 100.000000 4044 101 106 107 109 111 112 115 116 119 120 121 126 130 131 132 133 134 135 140 141 142 143 144 145 146 147 148 149 150 153 157 159 160 162 169 171 181 184
184 -67.379133 157.716802 272.249795 -116.589332 -20.693481 -76.293533 100.000000 2990 101 103 106 107 109 112 115 116 120 121 130 132 133 135 140 141 144 148 154 155 156 157 159 160 162 169 171 179 181 183
827 20.293650 10.448293 9.699148 -68.632993 38.905608 70.208202 100.000000 2 104 116 124 125 126 127 128 129 136 138 142 143 145

image


#2

Do you have your camera in the default camera position (forward facing) or do you have it in the upward facing position? Does it match what you have in the robot model?


#3

The camera came mounted . I never dismounted the camera. i merely tried another ribbon and then put back the default ribbon. so yes, i’m assuming the camera position is ok. at least it’s how it was made in factory.
the camera looks forward, in a 30~45 degrees fashion. it’s not the location where it tooks vertically directly at the ceiling (i’m guessing in such case i would have to make adjustments to the urdf or the like

do you need a photo?

about the robot model and the ros files in general. my policy until now has been to observe things and make sure i understand prior to make modifications. My expectation was that in the latest pi image (downloaded about 10 days ago), the declaration of the robot would match whatever was physically in the box. And i didn’t spot anything in the documentation refering to such adjustments.

for the record, the followme example worked out-of-the-box.


#4

i’m stuck here.

From the look of it in urdf/xacro, by default the cam is in forward position (and not upward)

ubuntu@ubiquityrobot:/opt/ros/kinetic/share/magni_description/launch$ cat description.launch
<launch>
  <arg name="raspicam_mount" default="forward"/>
  [...]
</launch>
ubuntu@ubiquityrobot:/opt/ros/kinetic/share/magni_description/launch$ cat ../urdf/magni.urdf.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="magni">
  <xacro:arg name="raspicam_mount" default="forward"/>
  <xacro:property name="raspicam_mount" value="$(arg raspicam_mount)"/>
  [...]
  <!-- Define raspicam based on the raspicam_mount argument -->
  <xacro:if value="${raspicam_mount == 'forward'}">
    <xacro:raspicam>
      <origin xyz="0.035 0.085 0.14" rpy="1.15191731 0 1.5707"/>
    </xacro:raspicam>
  </xacro:if>
  <xacro:if value="${raspicam_mount == 'upward'}">
    <xacro:raspicam>
      <origin xyz="0.035 0.15 0.14" rpy="0 0 -1.5707"/>
    </xacro:raspicam>
  </xacro:if>

</robot>
ubuntu@ubiquityrobot:/opt/ros/kinetic/share/magni_description/launch$

#5

Hey Alexis,

Can you please try removing the map, and trying to build it again?
To remove the map, stop the fiducials launch then rm /home/ubuntu/.ros/slam/map.txt

Are all your fiducials the same size / the default size?

Rohan


#6

OK let’s try this.

  1. Shut down anything that is running fiducials of any kind, perhaps even restart your robot.
  2. Delete map.txt
  3. You are starting with a very large number of fiducials (more than 80). Try doing fiducial SLAM again in a place with only 2 maybe 3 fiducials. Make sure those fiducials are completely flat, make sure there are no duplicates, and see if that works. We generally mount our fiducials on foam core to keep them flat, because it gives the best results and most print shops will do this for you, although there are many methods that can be used to get a flat fiducial.
  4. If it can work with 2 or 3 try to monitor the map in RViz and drive the robot so that it can see 2-3 fiducials in your big fiducial field. Then expand to say 10-20 and then eventually the whole field - try to see if there is a particular fiducial that seems to be causing problems. Make sure that this fiducial is flat and isn’t a duplicate.
  5. See if as you expand the fiducial field the fiducial map looks bad from the start and is always bad, looks bad from the start but gets better, looks good from the start and then gets worse.
  6. Check to see the error values that the system is reporting for the fiducials. You can do this in RViz or a rostopic echo. The error value on each fiducial should be on the order of a few cm at most. If the reported error value is higher than a few cm, then we have a different class of problems to the problems that are there if the error is good.

If the error is more than a few cm then the system is struggling to precisely figure out where your fiducials actually are, or its incorrectly assigning error values. This can be fixed by actions that tend to improve the sensing and accuracy and we’d look at things like have you got the camera in the correct high res mode, with the optimal lighting parameters, etc.

If the errors all look good but the map is still wrong, then the system thinks its doing thing correctly, but obviously isn’t, we’d look at things like camera orientation making sure our transform is correct etc.


#7

I got my fiducials printed on adhesive A4 paper (typically for big labels). I used an office printer (Lexmark T640). Then we put them directly on the walls and ceiling (it’s got an adhesive side) making sure they would be nice, clean and flat. I think this part is ok. have a look at two examples below.
There are no mirror in the room (nothing seriously reflective). we made sure there would be no fiducial on screens as well.

Assuming we want to have magni making a 100 meters single trip (not in this location), and we want to see at least 2 fiducials at all time, then i’ll have to deploy at least 200~300 fiducials depending on the required density (an uncertainty i am trying to answer with the current test).

i deleted the map. put magni in front of only 4 fiducials. stop/start the magni-base service.
then started roslaunch magni_nav aruco.launch without moving or touching the magni at all.

The 4 fiducials are all detected. their location and orientation make sense compared to the magni’s location.
What’s weird is that even though nothing’s moving in real life, in rviz both the 4 fiducials and the magni are slowly kinda drifting. I made the experient to delete the map and restart and observed a similar behavior

below is a typical output during that phase

[ INFO] [1534525128.635235250]: Estimate of 125 2.455556 1.365275 2.496617 err 0.094701 0.000000 var 0.095398
[ INFO] [1534525128.635396397]: Estimate of 127 2.711303 0.402771 2.696062 err 0.002222 0.000000 var 0.002919
[ INFO] [1534525128.635555772]: Estimate of 129 2.695059 -0.865489 2.761893 err 0.006681 0.000000 var 0.007379
[ INFO] [1534525128.635741137]: Estimate of 126 2.525718 0.284774 1.830682 err 0.000018 0.000000 var 0.000715
[ INFO] [1534525128.776640280]: Got image 642
[ INFO] [1534525129.250414660]: Detected 4 markers
[ INFO] [1534525129.259491046]: Detected id 125 T 1.20 1.00 2.92 R 2.04 -0.07 0.01
[ INFO] [1534525129.259641672]: angle 2.043229 axis 0.999340 -0.036167 0.003393
[ INFO] [1534525129.260826624]: Detected id 127 T 0.22 1.04 3.18 R 0.06 -3.08 -0.68
[ INFO] [1534525129.261098083]: angle 3.158289 axis 0.018266 -0.976352 -0.215415
[ INFO] [1534525129.261827148]: Detected id 129 T -1.05 1.00 3.15 R 0.00 3.07 0.70
[ INFO] [1534525129.267081230]: angle 3.150471 axis 0.000261 0.974841 0.222900
[ INFO] [1534525129.267868472]: Detected id 126 T 0.18 0.24 2.79 R -0.05 -3.03 -0.70
[ INFO] [1534525129.268128317]: angle 3.113839 axis -0.014649 -0.974413 -0.224288
[ INFO] [1534525129.283740090]: Updating map with 4 observations. Map has 4 fiducials
[ INFO] [1534525129.283895508]: Pose 125 -0.446952 -0.165512 1.078067 1.444188 0.153218 1.590876 0.024644
[ INFO] [1534525129.283972383]: Pose 127 -0.131353 -0.011749 0.942189 1.330002 0.096258 1.580038 0.003050
[ INFO] [1534525129.284046342]: Pose 129 -0.150874 -0.014112 0.947793 1.336760 0.100028 1.577676 0.007996
[ INFO] [1534525129.284114311]: Pose 126 -0.123213 -0.004570 0.933755 1.325668 0.094411 1.578494 0.001103
[ INFO] [1534525129.284181498]: Pose ALL -0.136976 -0.011770 0.941168 1.331090 0.097068 1.579042 0.000714
[ INFO] [1534525129.284283530]: camera   -0.136976 -0.011770 0.941168 0.000714
[ INFO] [1534525129.284340665]: Pose b_l -0.195553 -0.109606 0.818503 0.000714
[ INFO] [1534525129.284418166]: odom   0.000000 0.000000 0.000000
[ INFO] [1534525129.284506708]: Finished frame

[ INFO] [1534525129.284573323]: Estimate of 125 2.453869 1.363694 2.481748 err 0.024644 0.000000 var 0.025359
[ INFO] [1534525129.284824053]: Estimate of 127 2.702301 0.397737 2.680458 err 0.002050 0.000000 var 0.002765
[ INFO] [1534525129.284970668]: Estimate of 129 2.688524 -0.871229 2.754298 err 0.006996 0.000000 var 0.007711
[ INFO] [1534525129.285064574]: Estimate of 126 2.509022 0.276331 1.816992 err 0.000103 0.000000 var 0.000818
[ INFO] [1534525129.420847658]: Got image 648
[ INFO] [1534525129.902288837]: Detected 4 markers
[ INFO] [1534525129.908648912]: Detected id 125 T 1.19 1.00 2.91 R 2.04 -0.07 0.01
[ INFO] [1534525129.908999069]: angle 2.040981 axis 0.999357 -0.035463 0.005311
[ INFO] [1534525129.909487977]: Detected id 127 T 0.22 1.04 3.18 R 0.06 -3.08 -0.68
[ INFO] [1534525129.910058552]: angle 3.157019 axis 0.018198 -0.976265 -0.215812
[ INFO] [1534525129.910564023]: Detected id 129 T -1.05 1.00 3.15 R 0.00 3.07 0.70
[ INFO] [1534525129.910815274]: angle 3.148823 axis 0.000771 0.974919 0.222559
[ INFO] [1534525129.911377880]: Detected id 126 T 0.18 0.24 2.79 R -0.04 -3.04 -0.70
[ INFO] [1534525129.911634443]: angle 3.115602 axis -0.014178 -0.974498 -0.223947
[ INFO] [1534525129.933737855]: Updating map with 4 observations. Map has 4 fiducials
[ INFO] [1534525129.933893324]: Pose 125 -0.439657 -0.168197 1.092874 1.446793 0.156179 1.592621 0.021124
[ INFO] [1534525129.933967126]: Pose 127 -0.132780 -0.008323 0.938704 1.329377 0.096313 1.578487 0.004446
[ INFO] [1534525129.934040252]: Pose 129 -0.153543 -0.016129 0.949463 1.336977 0.098688 1.579497 0.008888
[ INFO] [1534525129.934109262]: Pose 126 -0.132937 -0.014043 0.939774 1.329533 0.096851 1.580539 0.001126
[ INFO] [1534525129.934174627]: Pose ALL -0.146140 -0.018951 0.946136 1.334498 0.099149 1.580407 0.000786
[ INFO] [1534525129.934261919]: camera   -0.146140 -0.018951 0.946136 0.000786
[ INFO] [1534525129.934319523]: Pose b_l -0.205029 -0.117106 0.823875 0.000786
[ INFO] [1534525129.934391815]: odom   0.000000 0.000000 0.000000
[ INFO] [1534525129.934471243]: Finished frame

[ INFO] [1534525129.934534993]: Estimate of 125 2.440678 1.358162 2.470377 err 0.021124 0.000000 var 0.021910
[ INFO] [1534525129.934723848]: Estimate of 127 2.699591 0.397410 2.675528 err 0.003446 0.000000 var 0.004232
[ INFO] [1534525129.934827494]: Estimate of 129 2.687682 -0.872080 2.752422 err 0.007888 0.000000 var 0.008673
[ INFO] [1534525129.934917807]: Estimate of 126 2.504358 0.273967 1.813049 err 0.000126 0.000000 var 0.000912
[ INFO] [1534525130.072527095]: Got image 655
[ INFO] [1534525130.557964124]: Detected 4 markers
[ INFO] [1534525130.564724200]: Detected id 125 T 1.20 1.00 2.92 R 3.26 -0.78 -1.09
[ INFO] [1534525130.565129827]: angle 3.525779 axis 0.924822 -0.220998 -0.309618
[ INFO] [1534525130.565691131]: Detected id 127 T 0.22 1.04 3.18 R 0.06 -3.08 -0.68
[ INFO] [1534525130.566207539]: angle 3.158404 axis 0.018052 -0.976544 -0.214561
[ INFO] [1534525130.566873166]: Detected id 129 T -1.06 1.00 3.15 R 0.00 3.07 0.70
[ INFO] [1534525130.567328012]: angle 3.149001 axis 0.000398 0.975065 0.221920
[ INFO] [1534525130.567985722]: Detected id 126 T 0.18 0.24 2.79 R -0.04 -3.03 -0.70
[ INFO] [1534525130.568333119]: angle 3.113599 axis -0.014147 -0.974378 -0.224470
[ INFO] [1534525130.583739528]: Updating map with 4 observations. Map has 4 fiducials
[ INFO] [1534525130.583912550]: Pose 125 3.522554 1.944964 -0.765344 -0.132747 -0.475682 1.922160 0.086757
[ INFO] [1534525130.583993123]: Pose 127 -0.140852 -0.013436 0.946719 1.333029 0.096577 1.580298 0.003389
[ INFO] [1534525130.584069008]: Pose 129 -0.156147 -0.016644 0.951498 1.338011 0.099483 1.579404 0.010031
[ INFO] [1534525130.584139998]: Pose 126 -0.138937 -0.011605 0.939818 1.330833 0.098075 1.578471 0.001014
[ INFO] [1534525130.584205936]: Pose ALL -0.110262 0.003845 0.928000 1.320609 0.092013 1.577833 0.000718
[ INFO] [1534525130.584307395]: camera   -0.110262 0.003845 0.928000 0.000718
[ INFO] [1534525130.584365624]: Pose b_l -0.167579 -0.093346 0.804231 0.000718
[ INFO] [1534525130.584441197]: odom   0.000000 0.000000 0.000000
[ INFO] [1534525130.584526354]: Finished frame

[ INFO] [1534525130.584594010]: Estimate of 125 2.463026 1.369688 2.503215 err 0.086757 0.000000 var 0.087475
[ INFO] [1534525130.584871772]: Estimate of 127 2.713239 0.404124 2.700246 err 0.002389 0.000000 var 0.003107
[ INFO] [1534525130.584992970]: Estimate of 129 2.698726 -0.866273 2.767486 err 0.009031 0.000000 var 0.009749
[ INFO] [1534525130.585088856]: Estimate of 126 2.528071 0.286971 1.833267 err 0.000014 0.000000 var 0.000732
[ INFO] [1534525130.724181014]: Got image 661
[ INFO] [1534525131.210201901]: Detected 4 markers
[ INFO] [1534525131.216680153]: Detected id 125 T 1.19 1.00 2.91 R 2.04 -0.07 0.02
[ INFO] [1534525131.216807549]: angle 2.042026 axis 0.999332 -0.035744 0.007597
[ INFO] [1534525131.217797136]: Detected id 127 T 0.22 1.04 3.19 R 0.06 -3.09 -0.68
[ INFO] [1534525131.218219429]: angle 3.162697 axis 0.019048 -0.976534 -0.214521
[ INFO] [1534525131.218724483]: Detected id 129 T -1.05 1.00 3.15 R 0.00 3.07 0.70
[ INFO] [1534525131.219110578]: angle 3.149315 axis 0.000229 0.975108 0.221732
[ INFO] [1534525131.219664278]: Detected id 126 T 0.18 0.24 2.79 R 0.05 3.09 0.71
[ INFO] [1534525131.220207718]: angle 3.168164 axis 0.014485 0.974456 0.224110
[ INFO] [1534525131.233721463]: Updating map with 4 observations. Map has 4 fiducials
[ INFO] [1534525131.233890839]: Pose 125 -0.430977 -0.178665 1.095790 1.446422 0.158248 1.596041 0.028072
[ INFO] [1534525131.233970996]: Pose 127 -0.134405 -0.025160 0.942906 1.330427 0.097563 1.583956 0.003399
[ INFO] [1534525131.234063340]: Pose 129 -0.151725 -0.014625 0.950231 1.336744 0.099127 1.578933 0.007770
[ INFO] [1534525131.234134330]: Pose 126 -0.122991 -0.005743 0.935116 1.325832 0.093937 1.579525 0.001190
[ INFO] [1534525131.234203601]: Pose ALL -0.136870 -0.015763 0.942785 1.331236 0.097060 1.580818 0.000770
[ INFO] [1534525131.234447768]: camera   -0.136870 -0.015763 0.942785 0.000770
[ INFO] [1534525131.234521987]: Pose b_l -0.195292 -0.113701 0.820127 0.000770
[ INFO] [1534525131.234597144]: odom   0.000000 0.000000 0.000000
[ INFO] [1534525131.234680842]: Finished frame

[ INFO] [1534525131.234745946]: Estimate of 125 2.441011 1.358672 2.476793 err 0.028072 0.000000 var 0.028842
[ INFO] [1534525131.234983655]: Estimate of 127 2.706355 0.399414 2.684326 err 0.002399 0.000000 var 0.003169
[ INFO] [1534525131.235100062]: Estimate of 129 2.692567 -0.870643 2.756747 err 0.006770 0.000000 var 0.007540
[ INFO] [1534525131.235268552]: Estimate of 126 2.510427 0.277219 1.818798 err 0.000190 0.000000 var 0.000960
[ INFO] [1534525131.384827780]: Got image 668
[ INFO] [1534525131.835319688]: Detected 4 markers
[ INFO] [1534525131.842809090]: Detected id 125 T 1.19 1.00 2.91 R 3.26 -0.78 -1.09
[ INFO] [1534525131.842942580]: angle 3.529914 axis 0.924711 -0.221869 -0.309328
[ INFO] [1534525131.843333519]: Detected id 127 T 0.22 1.04 3.19 R 0.06 -3.08 -0.68
[ INFO] [1534525131.843437842]: angle 3.159096 axis 0.018830 -0.976458 -0.214882
[ INFO] [1534525131.843914718]: Detected id 129 T -1.05 1.00 3.15 R 0.00 3.07 0.70
[ INFO] [1534525131.844018625]: angle 3.150563 axis 0.000257 0.975221 0.221232
[ INFO] [1534525131.844427637]: Detected id 126 T 0.18 0.24 2.79 R -0.04 -3.04 -0.70
[ INFO] [1534525131.844523575]: angle 3.115923 axis -0.014159 -0.974326 -0.224696
[ INFO] [1534525131.883748662]: Updating map with 4 observations. Map has 4 fiducials
[ INFO] [1534525131.883898818]: Pose 125 3.532255 1.944897 -0.752863 -0.137421 -0.474252 1.925333 0.073899
[ INFO] [1534525131.883980329]: Pose 127 -0.134422 -0.011856 0.939863 1.329596 0.096924 1.579583 0.003438
[ INFO] [1534525131.884055746]: Pose 129 -0.150570 -0.010799 0.951916 1.337000 0.098819 1.577900 0.004811
[ INFO] [1534525131.884126059]: Pose 126 -0.128331 -0.013954 0.936447 1.327479 0.096182 1.581096 0.001005
[ INFO] [1534525131.884192934]: Pose ALL -0.099714 0.004469 0.924076 1.317046 0.090365 1.579190 0.000663
[ INFO] [1534525131.884278247]: camera   -0.099714 0.004469 0.924076 0.000663
[ INFO] [1534525131.884333455]: Pose b_l -0.156431 -0.092610 0.799943 0.000663
[ INFO] [1534525131.884852468]: odom   0.000000 0.000000 0.000000
[ INFO] [1534525131.884979551]: Finished frame

(what do the error/var values means? the error in meter? the variance?)

now trying to move the magni with keyboard teleop sloooowly, mostly turning around in a one square meter

I can see that the new fiducals are displayed faar away. the magni base is even outside the grid whereas it should have stayed in a single square…

image

it looks like the odometry is completely broken. I can understand there is a cumulative error, but that much ?
when i then stop i got the following map

ubuntu@ubiquityrobot:~$ cat /home/ubuntu/.ros/slam/map.txt
116 26.422023 -5.016355 6.053514 94.697722 -8.770909 175.345953 100.000000 1290 137 139 140
125 2.297874 1.281827 2.239493 -160.659158 11.319909 88.631216 0.000000 10 126 127 129 142 143 146 147
126 2.556723 0.321188 1.843734 79.703690 -6.218985 -88.044731 0.001000 1960 125 127 129 137 142 143 146 147
127 2.738654 0.412991 2.714184 79.513987 -2.687559 -90.070604 0.001000 1958 125 126 129 142 143 146 147
129 28.200734 -4.296289 5.645260 79.914508 -3.607384 -90.773831 100.000000 2310 125 126 127 137 139 142
137 27.568178 -5.080678 6.006340 95.259861 -9.297480 178.275416 100.000000 1666 116 126 129 139 140
139 26.129968 -5.076036 5.496371 93.694068 -7.590647 176.491715 100.000000 1364 116 129 137 140
140 24.999657 -4.963664 5.834785 93.554184 -8.886934 175.866453 100.000000 1348 116 137 139
142 2.761090 2.504591 2.572013 79.043695 -6.236499 -90.658765 0.001000 58 125 126 127 129 143 146 147
143 2.036800 2.397813 3.048106 169.159086 -5.395260 -90.546053 0.001000 18 125 126 127 142 146 147
146 2.202395 3.278615 2.974691 168.333198 -2.621915 -77.232941 0.001000 22 125 126 127 142 143 147
147 2.700053 3.617287 2.416155 125.018835 -31.447516 11.112167 5.540227 8 125 126 127 142 143 146

what’s the format? if it starts with id x y z
then 116, 129, 137, 139 must be wrong (cant be in the 26meter range. should be in the 2~3m range)

if i reboot magni, put it in another position, delete the map and start it all over again
image
now it does not some fiducials, but then even though the magni has not moved, the location the map origin is far away from the magni ???

i tried to display odometry data (cyan arrows), it looks ok as i move around the magni. but then very quickly the magni center moves far away from it’s real location. looks like the odometry data is simply ignored in the slam process.

image

It’s just too confusing for me. i’ll try again on monday. in another room where i tape only a few fiducials we’ll see.


#8

Hello!

Im having the same problem as you!

I´ve seted up some goals and the robot stops always in different positions! The Magni stops “close” from the predifined waypoint but always on different locations, i believe dat its caused by the drifting dat you are mentioning, as i am observing the same behavior.

Can we buy a more potent RaspberryPi Cam to replace the one it came with the magni? Could dat improve the image recognition of the fiducials? Because when i have the magni totaly mobilized and the fiducials as well on rviz you can saw the drifting!

With this problem im stuck on further developments as i cant define a robot to stop on a place and he stops at a different location~!

Another problem is the sonars, they just didnt work when you use the command “Go To waypointname”… the robot just samshes everything on is way :slight_smile:

awaiting for help from community and customer support from magni team!

thanks in advace


#9

I tried in a simpler setup => same behavior.
I tried with a second magni => same behavior.

we should be having soon a troubleshooting procedure to find root cause and fix it.

i’ll report progress here


#10

OK so there is an important bug fix that relates to this problem that was just released see:

https://forum.ubiquityrobotics.com/t/boot-bug-regarding-time-synchronization

to make sure you aren’t affected by this bug you need to make sure your realtime clock battery is in place.

Also we have a work-around in software (desirable but you should really make sure that the real time clock battery is in place in addition to installing the latest software) to get the software patch use sudo apt update; sudo apt upgrade to get the latest software.

We also have a comprehensive troubleshooting guide that systematically goes through all the elements of the fiducial navigation system. See this here:

https://forum.ubiquityrobotics.com/t/troubleshooting-procedure-for-fiducial-localization-problems


#11

I added a CR2032 mini battery (was spare for PCs) in magni02. with my voltmeter i can see that it’s charging so it seems just fine.
I made the apt update/upgrade (twice, as advised), reboot.
I continue to observe fiducials in rviz whose position and orientation are mistaken.

On the fiducials, I did measure 142mm x 142mm with a ruler instead of the expected 140x 140 (opening the pdf in inkscape does show 140mm so it must be my printer or printer settings, dunno).


I went through all the troubleshooting procedure. everything’s just fine.
the echo tf seems to indicate a camera position in “forward”. Looking through the launch files, it seems to be the default. however at step #10 the rviz camera screenshot seems to indicate that the camera is upwards?

next steps:

  • printing a fiducial with another printer (to check if i still get 142x142 or 140x140)
  • calibrate the camera? :cry:

#12

@alexis the troubleshooting seems to do the trick in my case, the drifting fiducials have disapeared and now they seem much more steady… but my fiducials set up is a little different from yours! im placing them on the walls instead of the ceilling! maybe try change the disposition to see if still happens to you!


#13

this is my current field of fiducials! Its a little square, ive defined 3 different waypoint and the robot almost respects to the limit the location ive established for those waypoints, so by now, so far so good, but need to test it on an way larger environment, since this is gona to be working on an 4000sq meters warehouse along with more robots from ubiquity i hope :slight_smile:


#14

thanks for the feedback.
I noticed that the fiducial.pdf format does not seem to be A4. could that explain the 140mm vs 142mm discrepency


#15

Mine ends exactly at 140mm but personaly i dont believe it will matter! But it worth the shot, try it!!


#16

Yes the size is for 8.5’ x 11 which is a standard paper size here in the US. We selected this one because in most cases a printer that is set up for 8.5 X 11 will not print an A4 page but a printer that is set for A4 will print 8.5 X 11.

2mm is a 1.4% difference and this is significant - consider a fiducial that is 5 meters away - all the distances estimated will be out by about 70mm sytematically. The fiducial algorithm looks at many features of the square and computes the square size from all of them. As such it can determine the size of the square to significantly better than 1 pixel accuracy.

Alexis rather than pulling down all the fiducials you’ve put up, finding an ideal printer etc. why don’t you just keep printing on the same printer, measure the fiducial dimension that you are getting accurately (e.g. using a pair of calipers rather than a steel ruler) and then adjusting the parameter for the fiducial size.

The fiducial is a parameter that can be set for more on this check out.

http://wiki.ros.org/aruco_detect


#17

I don’t think that the fiducial_len is dynamically reconfigurable.

The quickest way to test with the other length is to edit the file /opt/ros/kinetic/share/magni_nav/launch/aruco.launch and change line 17 to read:

<arg name="fiducial_len" value="0.142" />

I made an issue here to support A4 paper PDFs natively as well https://github.com/UbiquityRobotics/fiducials/issues/111

Rohan


#18

I´ve moved to the next stage of testing and i am facing some problems again, but maybe its my fault!

On this larger environment, almost 30fiducials glued to the wall(not the ceiling) on a 200sq meters room, and the robot and the fiducials starts to drift on the rviz…

I will try with the fiducials glued to the ceiling tomorow to see if the problem goes away!


#19

In general moving the camera to the upward facing position and having the fiducials on the ceiling is going to give the best results for navigation. Forward facing is good because you can follow objects etc. but if you want to optimize for navigation having the camera perpendicular to the direction of motion and looking at fiducials that are the same distance away from the floor is surely better.


#20

@davecrawley when you say “moving the camera to the upward facing position” do you mean that i need to physicaly rotate the camera or can i change that somewhere in the configuration?

Currently my camera is in this orientation! See attachment please!!camera_orientation2