UGV not move to target goal

[ZhenkunYin]

Hi @eliabntt, thanks for open sourcing your amazing job.

I'm pretty new to this area and if you can give me any ideas about my issues, I will be really appreciate.

I tested this algorithm on Ubuntu 20.04 with ROS Noetic. It was compiled successfully and can be run under my system. But everytime after it determined the target, it always showed "ROBOT IS INVALID", like shown below, and it won't move.

It moved a little bit only when robotino_mpc showed "[ERROR] [1667120842.856498300, 244.120000000]: MOVE AWAY".

From fsm:

[ INFO] [1667120838.623885998, 240.140000000]: Received 6 goals.
[ INFO] [1667120838.623949493, 240.140000000]: is goal empty? 0
[ INFO] [1667120838.623978323, 240.140000000]: WE HAVE SOMETHING
[ INFO] [1667120838.646793222, 240.160000000]: get best path among 0
[ WARN] [1667120838.865163544, 240.370000000]: Empty current plan, retry. Failed 6 plans.
[ INFO] [1667120838.865219609, 240.370000000]: Got new Goal
[ INFO] [1667120839.133167287, 240.620000000]: Received 5 goals.
[ INFO] [1667120839.133236803, 240.620000000]: is goal empty? 0
[ INFO] [1667120839.173304259, 240.660000000]: Best path identified toward
[ INFO] [1667120839.173383452, 240.660000000]: x: 2.70065
y: -3.07662
z: 0

[ INFO] [1667120839.173439095, 240.660000000]: Got new Goal
[ WARN] [1667120839.257242694, 240.740000000]: ROBOT IS  INVALID. Number failures = 5

And in the active_node, it always showed get 0 utility and 0 entropy, like shown as below:

------------------------------------------------------------------
[ INFO] [1667120838.864799288, 240.370000000]: FOR kind of cost 7 we have: 
[ INFO] [1667120838.864852699, 240.370000000]: We get 0 utility
[ INFO] [1667120838.864884093, 240.370000000]: We get 0 entropy
[ INFO] [1667120838.864912923, 240.370000000]: Toward -1

[ INFO] [1667120842.848955562, 244.120000000]: req received
[ INFO] [1667120842.855480606, 244.120000000]: OccupancyGrid map call successfull

The robotino_mpc will show:

[ WARN] [1667120839.233826914, 240.720000000]: Nonlinear MPC: Solver failed with status: 0
[ WARN] [1667120839.233936635, 240.720000000]: Error Successful return
[ WARN] [1667120839.234005375, 240.720000000]: reinitializing...

And the best_heading always shows:

[ WARN] [1667120616.187923805, 33.710000000]: kp3D is empty

It was tested on 'cafe', and if you have any ideas about this issue, please let me know. Any replies will be appreciate.

Thanks!

[eliabntt]

Did you edit anything?

Can you share a picture/video of when this happens?

Seems likely that the robot is not in a valid position (e.g. nearby an obstacle or something). Also, please check that everything is actually running and looping (e.g. rtabmap is receiving images, /odometry/filtered is publishing.. ). I just compiled everything on Noetic last week and it was working.

MOVE_AWAY is a forceful command that we send to the robot. When it detects that it is stuck it tries to move away from the nearby obstacles following a vector. This movement is very small.

You could try not launching the mpc and send cmd_vel commands with the teleop package/command line to see if the robot moves and rtabmap builds a map

[ZhenkunYin]

Thanks for your reply. I only edit a "C++11" to "C++14" during compiling.

I have shared my video to dropbox, the link has been shown as below. https://www.dropbox.com/s/p5zt9lhsmejey33/selfrotate10-2022-10-30_21.18.14.mp4?dl=0

I think rtabmap part should be fine, because robot can move and map built successfully.

[eliabntt]

Strange, which one? Did you checkout the noetic branch? Or are you on another branch?

I will check the video tomorrow.

Does it do the first self Rotation?

On Sun, 30 Oct 2022, 11:32 ZhenkunYin, @.***> wrote:

Thanks for your reply. I only edit a "C++11" to "C++14" during compiling.

I have shared my video to dropbox, the link has been shown as below.

https://www.dropbox.com/s/p5zt9lhsmejey33/selfrotate10-2022-10-30_21.18.14.mp4?dl=0

I think rtabmap part should be fine, because robot can move and map built successfully.

— Reply to this email directly, view it on GitHub https://github.com/eliabntt/irotate_active_slam/issues/5#issuecomment-1296205505, or unsubscribe https://github.com/notifications/unsubscribe-auth/AEXDUJS4WMQNFDOXKL4BEULWFZFDHANCNFSM6AAAAAARSGQ4VQ . You are receiving this because you were mentioned.Message ID: @.***>

[ZhenkunYin]

I am using noetic branch.

Yes, it does. And it just repeat over and over again, and also, reset the rtabmap after a while.

Thanks for your help!

[eliabntt]

Hi there, so I got hold of a pc for a short while. I downloaded the code from the repo, updated the CMakeLists of the camera heading, built everything from scratch and it worked.

I also looked at your video but you're too quick launching the commands it's hard to get anything and grasp if there are errors. If you have the time please try and send me the logs.

Also please try to rebuild the nmpc code (see here and afterwards the catkin workspace.

[ WARN] [1667130550.177991241, 102.310000000]: using old tf
[ WARN] [1667130550.178100790, 102.310000000]: TF to MSG: Quaternion Not Properly Normalized
[ WARN] [1667130550.178142565, 102.310000000]: TF to MSG: Quaternion Not Properly Normalized
[ INFO] [1667130550.178222652, 102.310000000]: opt 360 wanted 360 sought 360 dist 0 0 current 360

This is the expected output of the MPC BEFORE you launch rtabmap.

As soon as you launch rtabmap it should switch to what you see.

Rtabmap output seems fine, thus I guess ekfs and images are working correctly.

Also, please share svg/pngs of the node graphs after each command. (rosrun rqt_graph rqt_graph -> chose Nodes/Topics (active) in the dropdown list, use / and / in the next two boxes. Use Group: 8 and check every checkbox on the first row, uncheck every checkbox in the second row Hide) After you run each one of the launch commands click on the refresh blue arrow :)

This should help understand if everything gets launched.

[ZhenkunYin]

Really appreciate your patience!

I believe I have rebuilt the nmpc, and I get warningd, shown as below, when I cmake. I think it won't be a problem, because I can successfully make.

CMake Deprecation Warning at CMakeLists.txt:161 (CMAKE_POLICY):
  The OLD behavior for policy CMP0026 will be removed from a future version
  of CMake.

  The cmake-policies(7) manual explains that the OLD behaviors of all
  policies are deprecated and that a policy should be set to OLD only under
  specific short-term circumstances.  Projects should be ported to the NEW
  behavior and not rely on setting a policy to OLD.

CMake Deprecation Warning at CMakeLists.txt:164 (CMAKE_POLICY):
  The OLD behavior for policy CMP0042 will be removed from a future version
  of CMake.

  The cmake-policies(7) manual explains that the OLD behaviors of all
  policies are deprecated and that a policy should be set to OLD only under
  specific short-term circumstances.  Projects should be ported to the NEW
  behavior and not rely on setting a policy to OLD.

-- Configuring done
-- Generating done
-- Build files have been written to: /home/zhenkun/Dev/ACADOtoolkit/build

I have put node graphs and new video into this dropbox: https://www.dropbox.com/scl/fo/xccocqesobxqt7t583rxu/h?dl=0&rlkey=d4ev0gggyvzjpov4erlbidyq6

and logs have been attached as below: frontier_exploration-1-stdout.log imu_republish-8.log imu_republish-8-stdout.log master.log move_base-5-stdout.log roslaunch-zhenkun-Surface-Laptop-2-111186.log roslaunch-zhenkun-Surface-Laptop-2-111720.log roslaunch-zhenkun-Surface-Laptop-2-111890.log roslaunch-zhenkun-Surface-Laptop-2-112666.log roslaunch-zhenkun-Surface-Laptop-2-113341.log roslaunch-zhenkun-Surface-Laptop-2-113881.log rosout.log rosout-1-stdout.log rrbot-robot_state_publisher-7-stdout.log rtabmap-icp_odometry-3-stdout.log spawn_gazebo_model-6.log standalone_converter-6-stdout.log

Again, thanks for your help!

[eliabntt]

Please check your move base installation. There is something wrong there. As you see from the rtabmap png that you shared it is completely detached.

On Sun, Oct 30, 2022 at 2:12 PM ZhenkunYin @.***> wrote:

Really appreciate your patience!

I believe I have rebuilt the nmpc, and I get warningd, shown as below, when I cmake. I think it won't be a problem, because I can successfully make.

CMake Deprecation Warning at CMakeLists.txt:161 (CMAKE_POLICY): The OLD behavior for policy CMP0026 will be removed from a future version of CMake.

The cmake-policies(7) manual explains that the OLD behaviors of all policies are deprecated and that a policy should be set to OLD only under specific short-term circumstances. Projects should be ported to the NEW behavior and not rely on setting a policy to OLD.

CMake Deprecation Warning at CMakeLists.txt:164 (CMAKE_POLICY): The OLD behavior for policy CMP0042 will be removed from a future version of CMake.

The cmake-policies(7) manual explains that the OLD behaviors of all policies are deprecated and that a policy should be set to OLD only under specific short-term circumstances. Projects should be ported to the NEW behavior and not rely on setting a policy to OLD.

-- Configuring done -- Generating done -- Build files have been written to: /home/zhenkun/Dev/ACADOtoolkit/build

I have put node graphs and new video into this dropbox:

https://www.dropbox.com/scl/fo/xccocqesobxqt7t583rxu/h?dl=0&rlkey=d4ev0gggyvzjpov4erlbidyq6

and logs have been attached as below: frontier_exploration-1-stdout.log https://github.com/eliabntt/irotate_active_slam/files/9895859/frontier_exploration-1-stdout.log imu_republish-8.log https://github.com/eliabntt/irotate_active_slam/files/9895860/imu_republish-8.log imu_republish-8-stdout.log https://github.com/eliabntt/irotate_active_slam/files/9895861/imu_republish-8-stdout.log master.log https://github.com/eliabntt/irotate_active_slam/files/9895862/master.log move_base-5-stdout.log https://github.com/eliabntt/irotate_active_slam/files/9895863/move_base-5-stdout.log roslaunch-zhenkun-Surface-Laptop-2-111186.log https://github.com/eliabntt/irotate_active_slam/files/9895864/roslaunch-zhenkun-Surface-Laptop-2-111186.log roslaunch-zhenkun-Surface-Laptop-2-111720.log https://github.com/eliabntt/irotate_active_slam/files/9895865/roslaunch-zhenkun-Surface-Laptop-2-111720.log roslaunch-zhenkun-Surface-Laptop-2-111890.log https://github.com/eliabntt/irotate_active_slam/files/9895866/roslaunch-zhenkun-Surface-Laptop-2-111890.log roslaunch-zhenkun-Surface-Laptop-2-112666.log https://github.com/eliabntt/irotate_active_slam/files/9895867/roslaunch-zhenkun-Surface-Laptop-2-112666.log roslaunch-zhenkun-Surface-Laptop-2-113341.log https://github.com/eliabntt/irotate_active_slam/files/9895868/roslaunch-zhenkun-Surface-Laptop-2-113341.log roslaunch-zhenkun-Surface-Laptop-2-113881.log https://github.com/eliabntt/irotate_active_slam/files/9895869/roslaunch-zhenkun-Surface-Laptop-2-113881.log rosout.log https://github.com/eliabntt/irotate_active_slam/files/9895870/rosout.log rosout-1-stdout.log https://github.com/eliabntt/irotate_active_slam/files/9895871/rosout-1-stdout.log rrbot-robot_state_publisher-7-stdout.log https://github.com/eliabntt/irotate_active_slam/files/9895872/rrbot-robot_state_publisher-7-stdout.log rtabmap-icp_odometry-3-stdout.log https://github.com/eliabntt/irotate_active_slam/files/9895873/rtabmap-icp_odometry-3-stdout.log spawn_gazebo_model-6.log https://github.com/eliabntt/irotate_active_slam/files/9895874/spawn_gazebo_model-6.log standalone_converter-6-stdout.log https://github.com/eliabntt/irotate_active_slam/files/9895875/standalone_converter-6-stdout.log

Again, thanks for your help!

— Reply to this email directly, view it on GitHub https://github.com/eliabntt/irotate_active_slam/issues/5#issuecomment-1296255681, or unsubscribe https://github.com/notifications/unsubscribe-auth/AEXDUJW6B7IPQFBXG3UQCHDWFZX5LANCNFSM6AAAAAARSGQ4VQ . You are receiving this because you were mentioned.Message ID: @.***>

[eliabntt]

You can try and launch it stand-alone. Just work it out from the rtabmap launcher.

On Sun, Oct 30, 2022 at 3:12 PM Elia Bonetto @.***> wrote:

Please check your move base installation. There is something wrong there. As you see from the rtabmap png that you shared it is completely detached.

On Sun, Oct 30, 2022 at 2:12 PM ZhenkunYin @.***> wrote:

Really appreciate your patience!

I believe I have rebuilt the nmpc, and I get warningd, shown as below, when I cmake. I think it won't be a problem, because I can successfully make.

CMake Deprecation Warning at CMakeLists.txt:161 (CMAKE_POLICY): The OLD behavior for policy CMP0026 will be removed from a future version of CMake.

The cmake-policies(7) manual explains that the OLD behaviors of all policies are deprecated and that a policy should be set to OLD only under specific short-term circumstances. Projects should be ported to the NEW behavior and not rely on setting a policy to OLD.

CMake Deprecation Warning at CMakeLists.txt:164 (CMAKE_POLICY): The OLD behavior for policy CMP0042 will be removed from a future version of CMake.

The cmake-policies(7) manual explains that the OLD behaviors of all policies are deprecated and that a policy should be set to OLD only under specific short-term circumstances. Projects should be ported to the NEW behavior and not rely on setting a policy to OLD.

-- Configuring done -- Generating done -- Build files have been written to: /home/zhenkun/Dev/ACADOtoolkit/build

I have put node graphs and new video into this dropbox:

https://www.dropbox.com/scl/fo/xccocqesobxqt7t583rxu/h?dl=0&rlkey=d4ev0gggyvzjpov4erlbidyq6

and logs have been attached as below: frontier_exploration-1-stdout.log https://github.com/eliabntt/irotate_active_slam/files/9895859/frontier_exploration-1-stdout.log imu_republish-8.log https://github.com/eliabntt/irotate_active_slam/files/9895860/imu_republish-8.log imu_republish-8-stdout.log https://github.com/eliabntt/irotate_active_slam/files/9895861/imu_republish-8-stdout.log master.log https://github.com/eliabntt/irotate_active_slam/files/9895862/master.log move_base-5-stdout.log https://github.com/eliabntt/irotate_active_slam/files/9895863/move_base-5-stdout.log roslaunch-zhenkun-Surface-Laptop-2-111186.log https://github.com/eliabntt/irotate_active_slam/files/9895864/roslaunch-zhenkun-Surface-Laptop-2-111186.log roslaunch-zhenkun-Surface-Laptop-2-111720.log https://github.com/eliabntt/irotate_active_slam/files/9895865/roslaunch-zhenkun-Surface-Laptop-2-111720.log roslaunch-zhenkun-Surface-Laptop-2-111890.log https://github.com/eliabntt/irotate_active_slam/files/9895866/roslaunch-zhenkun-Surface-Laptop-2-111890.log roslaunch-zhenkun-Surface-Laptop-2-112666.log https://github.com/eliabntt/irotate_active_slam/files/9895867/roslaunch-zhenkun-Surface-Laptop-2-112666.log roslaunch-zhenkun-Surface-Laptop-2-113341.log https://github.com/eliabntt/irotate_active_slam/files/9895868/roslaunch-zhenkun-Surface-Laptop-2-113341.log roslaunch-zhenkun-Surface-Laptop-2-113881.log https://github.com/eliabntt/irotate_active_slam/files/9895869/roslaunch-zhenkun-Surface-Laptop-2-113881.log rosout.log https://github.com/eliabntt/irotate_active_slam/files/9895870/rosout.log rosout-1-stdout.log https://github.com/eliabntt/irotate_active_slam/files/9895871/rosout-1-stdout.log rrbot-robot_state_publisher-7-stdout.log https://github.com/eliabntt/irotate_active_slam/files/9895872/rrbot-robot_state_publisher-7-stdout.log rtabmap-icp_odometry-3-stdout.log https://github.com/eliabntt/irotate_active_slam/files/9895873/rtabmap-icp_odometry-3-stdout.log spawn_gazebo_model-6.log https://github.com/eliabntt/irotate_active_slam/files/9895874/spawn_gazebo_model-6.log standalone_converter-6-stdout.log https://github.com/eliabntt/irotate_active_slam/files/9895875/standalone_converter-6-stdout.log

Again, thanks for your help!

— Reply to this email directly, view it on GitHub https://github.com/eliabntt/irotate_active_slam/issues/5#issuecomment-1296255681, or unsubscribe https://github.com/notifications/unsubscribe-auth/AEXDUJW6B7IPQFBXG3UQCHDWFZX5LANCNFSM6AAAAAARSGQ4VQ . You are receiving this because you were mentioned.Message ID: @.***>

[ZhenkunYin]

Thanks for your suggestions. I finally make it to run!

I reinstalled the rtabmap, and got the same problem with https://github.com/eliabntt/irotate_active_slam/issues/4 . I find that the main reason is that I also installed rtabmap from apt, so I removed it and reinstall the rtabmap again, all the problem is fixed.

And could you tell me how to run the independent camera? Sorry to bother you again.

Thanks very much for your help!

[eliabntt]

I reinstalled the rtabmap, and got the same problem with https://github.com/eliabntt/irotate_active_slam/issues/4 . I find that the main reason is that I also installed rtabmap from apt, so I removed it and reinstall the rtabmap again, all the problem is fixed.

Thanks. I'll add that to the readme

And could you tell me how to run the independent camera? Sorry to bother you again.

Changing this should be sufficient iirc. The parameter controls the maximum speed of the camera allowed by the NMPC. If the speed is 0 clearly it resorts to make rotate only the robot while if it's != 0 it will plan the optimal rotation according to the kinematic definition here

Note: if you change the config file you need to rebuild! Otherwise it is possible to use either dynamic reconfigure or the yaml file.

[eliabntt]

So I suggest that maybe it's better to also mention remove the installed rtabmap before compiling for some beginner, like me.

BTW, it was mentioned on the INSTALL.md file. I made it clearer now. Thanks!

[eliabntt]

Hi @ZhenkunYin, If you test the independent camera movement on noetic, please be aware that I cannot guarantee any performance of rtabmap. Last time I tested was with this edit applied and the rtabmap (the main code) checked out at 5d200a0. I am curious to know if you encounter any issue at all with the new rtabmap. No edits should be necessary since the developers there addressed the issue of not updating the world when the camera is rotating independently.

[ZhenkunYin]

Thanks for noticing. Its performance looks good in my case, no any issue raised during mapping. I will also test it on real robot, and will let you know how it performs.