Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

UGV not move to target goal #5

Closed
ZhenkunYin opened this issue Oct 30, 2022 · 13 comments
Closed

UGV not move to target goal #5

ZhenkunYin opened this issue Oct 30, 2022 · 13 comments

Comments

@ZhenkunYin
Copy link

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
Copy link
Owner

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
Copy link
Author

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
Copy link
Owner

eliabntt commented Oct 30, 2022 via email

@ZhenkunYin
Copy link
Author

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
Copy link
Owner

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
Copy link
Author

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
Copy link
Owner

eliabntt commented Oct 30, 2022 via email

@eliabntt
Copy link
Owner

eliabntt commented Oct 30, 2022 via email

@ZhenkunYin
Copy link
Author

ZhenkunYin commented Oct 31, 2022 •
edited

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

I reinstalled the rtabmap, and got the same problem with #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!

@ZhenkunYin ZhenkunYin reopened this Oct 31, 2022
@eliabntt
Copy link
Owner

eliabntt commented Oct 31, 2022 •
edited

I reinstalled the rtabmap, and got the same problem with #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 eliabntt mentioned this issue Oct 31, 2022
Closed
@eliabntt
Copy link
Owner

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
Copy link
Owner

eliabntt commented Nov 1, 2022 •
edited

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
Copy link
Author

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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@eliabntt @ZhenkunYin