search

argenos / sesame_multi_robot_navigation

0 stars 1 forks source link

Robots not moving with cmd_vel (TF's not setting up properly?) #1

Closed jrharbin-york closed 1 year ago

jrharbin-york commented 1 year ago

I have set up everything with ROS Noetic on a new install on Ubuntu 20.04. I can run the standard tutorials and in the tutorials, drive the Tiago robot around with cmd_vel as shown here: http://wiki.ros.org/Robots/TIAGo/Tutorials/motions/cmd_vel

In the scenario started with the following:

roslaunch exsce_ros scenario.launch scenario_id:=scenario_x

The 4 robots show in rviz (I have to click and unclick them to display them), but when sending commands to e.g. /tiago1/mobile_baser_controller/cmd_vel there is no response. Also the costmaps in blue/purple are not shown. This is probably due to lots of errors at the start related to the TF's:

ERROR] [1692701755.825361687, 3.806000000]: Could not find a connection between 'pmb1/map' and 'pmb1/base_footprint' because they are not part of the same tree.Tf has two or more unconnected trees.
[ERROR] [1692701755.826274040, 3.807000000]: Could not find a connection between 'tiago2/map' and 'tiago2/base_footprint' because they are not part of the same tree.Tf has two or more unconnected trees.
[ERROR] [1692701755.827272604, 3.808000000]: Could not find a connection between 'omni1/map' and 'omni1/base_footprint' because they are not part of the same tree.Tf has two or more unconnected trees.
[ERROR] [1692701755.873319428, 3.854000000]: Could not find a connection between 'tiago1/map' and 'tiago1/base_footprint' because they are not part of the same tree.Tf has two or more unconnected trees.
[ERROR] [1692701756.025887158, 4.006000000]: Could not find a connection between 'pmb1/map' and 'pmb1/base_footprint' because they are not part of the same tree.Tf has two or more unconnected trees.
[ERROR] [1692701756.026724031, 4.007000000]: Could not find a connection between 'tiago2/map' and 'tiago2/base_footprint' because they are not part of the same tree.Tf has two or more unconnected trees.
[ERROR] [1692701756.027479543, 4.008000000]: Could not find a connection between 'omni1/map' and 'omni1/base_footprint' because they are not part of the same tree.Tf has two or more unconnected trees.
[ERROR] [1692701756.073640249, 4.054000000]: Could not find a connection between 'tiago1/map' and 'tiago1/base_footprint' because they are not part of the same tree.Tf has two or more unconnected trees.
[ERROR] [1692701756.225976947, 4.206000000]: Could not find a connection between 'pmb1/map' and 'pmb1/base_footprint' because they are not part of the same tree.Tf has two or more unconnected trees.
[ERROR] [1692701756.226621359, 4.207000000]: Could not find a connection between 'tiago2/map' and 'tiago2/base_footprint' because they are not part of the same tree.Tf has two or more unconnected trees.
[ERROR] [1692701756.227339252, 4.208000000]: Could not find a connection between 'omni1/map' and 'omni1/base_footprint' because they are not part of the same tree.Tf has two or more unconnected trees.
[ERROR] [1692701756.273713847, 4.254000000]: Could not find a connection between 'tiago1/map' and 'tiago1/base_footprint' because they are not part of the same tree.Tf has two or more unconnected trees.

I also get the error below about the OmniDriveController; but I think this is expected since a component is not included in the public simulation? http://wiki.ros.org/Robots/TIAGo-OMNI-base/Tutorials/Installation/OMNI_Base_Simulation

[ERROR] [1692701759.497413602, 4.859000000]: Could not load controller 'mobile_base_controller' because controller type 'omni_drive_controller/OmniDriveController' does not exist.
[ERROR] [1692701759.497466672, 4.859000000]: Use 'rosservice call controller_manager/list_controller_types' to get the available types

roswtf reports:

Loaded plugin openni2_launch.wtf_plugin Loaded plugin tf.tfwtf No package or stack in the current directory

Static checks summary:

No errors or warnings

Beginning tests of your ROS graph. These may take a while... analyzing graph... ... done analyzing graph running graph rules... ... done running graph rules Traceback (most recent call last): File "/opt/ros/noetic/lib/python3/dist-packages/roswtf/init.py", line 224, in _roswtf_main p(ctx) File "/opt/ros/noetic/lib/python3/dist-packages/openni2_launch/wtf_plugin.py", line 114, in roswtf_plugin_online error_rule(r, r0, ctx) File "/opt/ros/noetic/lib/python3/dist-packages/openni2_launch/wtf_plugin.py", line 85, in sensor_notfound devices = _device_notfound_subproc( File "/opt/ros/noetic/lib/python3/dist-packages/openni2_launch/wtf_plugin.py", line 58, in _device_notfound_subproc for i in df.split('\n'): TypeError: a bytes-like object is required, not 'str' a bytes-like object is required, not 'str'

Aborting checks, partial results summary:

Found 2 warning(s). Warnings are things that may be just fine, but are sometimes at fault WARNING The following node subscriptions are unconnected:

Found 1 error(s).

ERROR The following nodes should be connected but aren't:

jrharbin-york commented 1 year ago

Actually the robots seem to move properly, if I publish to XXX/key_vel or XXX/nav_vel, it just needed larger values than expected for any motion to be observable. What are the units (is it degrees for angular rotation, and centimeters for x,y,z?)

Something is still wrong with the coordinate systems, I can see messages like this on the console:

[ WARN] [1692712639.121412615, 46.377000000]: The origin for the sensor at (-17.04, -40.14) is out of map bounds. So, the costmap cannot raytrace for it.
[ WARN] [1692712661.924783743, 47.577000000]: The origin for the sensor at (-35.75, 47.89) is out of map bounds. So, the costmap cannot raytrace for it.
[ WARN] [1692712664.056702664, 47.677000000]: The origin for the sensor at (71.24, 31.51) is out of map bounds. So, the costmap cannot raytrace for it.
[ WARN] [1692712680.414390463, 48.577000000]: The origin for the sensor at (-35.58, 47.83) is out of map bounds. So, the costmap cannot raytrace for it.
[ WARN] [1692712682.565976022, 48.677000000]: The origin for the sensor at (69.19, 39.95) is out of map bounds. So, the costmap cannot raytrace for it.
[ WARN] [1692712699.842475880, 49.577000000]: The origin for the sensor at (-32.65, 93.74) is out of map bounds. So, the costmap cannot raytrace for it.
[ WARN] [1692712714.316958399, 50.277000000]: The origin for the sensor at (71.28, 34.11) is out of map bounds. So, the costmap cannot raytrace for it.
[ WARN] [1692712719.871420525, 50.577000000]: The origin for the sensor at (-32.61, 93.97) is out of map bounds. So, the costmap cannot raytrace for it.
[ WARN] [1692712739.634235150, 51.577000000]: The origin for the sensor at (-34.55, 47.45) is out of map bounds. So, the costmap cannot raytrace for it.
[ WARN] [1692712758.823233534, 52.577000000]: The origin for the sensor at (-34.24, 47.22) is out of map bounds. So, the costmap cannot raytrace for it.
[ WARN] [1692712777.715778688, 53.577000000]: The origin for the sensor at (-32.56, 94.98) is out of map bounds. So, the costmap cannot raytrace for it.
argenos commented 1 year ago

If you cd into your catkin workspace, what is the output of wstool info -t src?

argenos commented 1 year ago

These in particular should be on a different branch than the official tiago tutorials:

Localname                     S SCM Version (Spec)            UID  (Spec)  URI  (Spec) [http(s)://...]
 ---------                     - --- --------------            -----------  ---------------------------
 tiago_simulation                git sesame-devel              1f9d17b6565c git@github.com:argenos/tiago_simulation.git
 tiago_moveit_config             git sesame-devel              5504cc891d5b git@github.com:argenos/tiago_moveit_config.git
 robot_pose                    M git sesame-devel              b8b7c9360378 git@github.com:argenos/robot_pose.git
 pal_navigation_cfg_public       git sesame-devel              f291d87408de git@github.com:argenos/pal_navigation_cfg_public.git
 hector_gazebo                   git sesame-devel              82514a7441cd git@github.com:argenos/hector_gazebo.git

I have no tf issues on my setup, but I also don't send command velocities to the robots directly, I only interface with them through move_base

jrharbin-york commented 1 year ago

This is the output of wstool info -t src below. In particular all the ones you mentioned seem to be the same branches and UIDs.

The only difference was I changed some lines in your rosinstall file to use e.g. https://github.com/argenos/tiago_simulation.git instead of git@github.com:argenos/tiago_simulation.git because it gave me permission errors.

Could this be a problem?

As I said in the second comment, the robots do move now under velocity commands, I just had to set velocities on key_vel or nav_vel instead, and use larger values.

I'll try setting move_base goals and robots and see if they work properly.

Thanks for your help.

 Localname                     S SCM Version (Spec)            UID  (Spec)  URI  (Spec) [http(s)://...]
 ---------                     - --- --------------            -----------  ---------------------------
 exsce_ros                       git main                      2e80bbca7d29 github.com/hbrs-sesame/exsce_ros.git
 waypoint_dispatcher             git main                      b6597fe27650 github.com/argenos/waypoint_dispatcher.git
 floorplan-dsl-environments      git main                      cec70afda3aa github.com/hbrs-sesame/floorplan-dsl-environments.git
 hector_gazebo                   git sesame-devel              82514a7441cd github.com/argenos/hector_gazebo.git
 sesame_multi_robot_navigation   git main                      3fcea757440b github.com/hbrs-sesame/sesame_multi_robot_navigation.git
 tiago_simulation                git sesame-devel              1f9d17b6565c github.com/argenos/tiago_simulation.git
 tiago_moveit_config             git sesame-devel              5504cc891d5b github.com/argenos/tiago_moveit_config.git
 robot_pose                      git sesame-devel              b8b7c9360378 github.com/argenos/robot_pose.git
 pal_navigation_cfg_public       git sesame-devel              f291d87408de github.com/argenos/pal_navigation_cfg_public.git
 tiago_tutorials                 git noetic-devel              7c6f86d10e58 github.com/pal-robotics/tiago_tutorials.git
 tiago_robot                     git kinetic-devel             3f0f25263146 github.com/pal-robotics/tiago_robot.git
 tiago_navigation                git kinetic-devel             0bedc705944b github.com/pal-robotics/tiago_navigation.git
 tiago_description_calibration   git indigo-devel              b608b8da030d github.com/pal-robotics/tiago_description_calibration.git
 tf_lookup                       git master                    b4f91d32d029 github.com/pal-robotics/tf_lookup.git
 teleop_tools                    git pal-overlay-kinetic-devel 9b42c179afa6 github.com/pal-robotics-forks/teleop_tools.git
 slam_gmapping                   git melodic-devel             0835d19c69da github.com/ros-perception/slam_gmapping.git
 simple_models_robot             git kinetic-devel             568a2f3c6f29 github.com/pal-robotics/simple_models_robot.git
 simple_grasping_action          git master                    f5e353c18407 github.com/pal-robotics/simple_grasping_action.git
 rviz_plugin_covariance          git kinetic-devel             b93e3ecc1411 github.com/pal-robotics/rviz_plugin_covariance.git
 ros_control                     git kinetic-devel             15fffa370b11 github.com/pal-robotics-forks/ros_control.git
 ros_controllers                 git kinetic-devel             d9b716905777 github.com/pal-robotics-forks/ros_controllers.git
 roboticsgroup_gazebo_plugins    git kinetic-devel             bdc61975f998 github.com/pal-robotics-forks/roboticsgroup_gazebo_plugins.git
 pmb2_simulation                 git kinetic-devel             c424da0d17a6 github.com/pal-robotics/pmb2_simulation.git
 pmb2_robot                      git kinetic-devel             912a89e26c43 github.com/pal-robotics/pmb2_robot.git
 pmb2_navigation                 git kinetic-devel             b651414cf218 github.com/pal-robotics/pmb2_navigation.git
 play_motion                     git kinetic-devel             7d8e0b5fdbb2 github.com/pal-robotics/play_motion.git
 pal_wsg_gripper                 git indigo-devel              ee542262a264 github.com/pal-robotics/pal_wsg_gripper
 pal_statistics                  git kinetic-devel             6086bfcfcff4 github.com/pal-robotics/pal_statistics.git
 pal_robotiq_gripper             git master                    4f1b047cc3c1 github.com/pal-robotics/pal_robotiq_gripper.git
 pal_python                      git melodic-devel             2e33adfb1edb github.com/pal-robotics/pal_python.git
 pal_navigation_sm               git master                    3e6ae1082a8f github.com/pal-robotics/pal_navigation_sm.git
 pal_msgs                        git indigo-devel              6fe5c7453951 github.com/pal-robotics/pal_msgs.git
 pal_hardware_interfaces         git indigo-devel              64da44a37714 github.com/pal-robotics/pal_hardware_interfaces.git
 pal_hardware_gazebo             git melodic-devel             320e67210e70 github.com/pal-robotics/pal_hardware_gazebo.git
 pal_gripper                     git kinetic-devel             981421243546 github.com/pal-robotics/pal_gripper.git
 pal_gazebo_worlds               git kinetic-devel             678e1d7675de github.com/pal-robotics/pal_gazebo_worlds.git
 pal_gazebo_plugins              git melodic-devel             5677a5ec7598 github.com/pal-robotics/pal_gazebo_plugins.git
 openslam_gmapping               git melodic-devel             96e52322e51b github.com/ros-perception/openslam_gmapping.git
 omni_base_simulation            git melodic-devel             8786436292e7 github.com/pal-robotics/omni_base_simulation.git
 omni_base_robot                 git melodic-devel             b19a41d52e34 github.com/pal-robotics/omni_base_robot.git
 omni_base_navigation            git melodic-devel             59214e38b3db github.com/pal-robotics/omni_base_navigation.git
 navigation_layers               git noetic                    8899b6e95041 github.com/DLu/navigation_layers.git
 humanoid_msgs                   git master                    6fcf3b6c54bb github.com/ahornung/humanoid_msgs.git
 hey5_description                git master                    6608818d3df3 github.com/pal-robotics/hey5_description.git
 head_action                     git indigo-devel              79e98a8e54b1 github.com/pal-robotics/head_action.git
 gazebo_ros_pkgs                 git noetic-devel              80b1ebef62f9 github.com/pal-robotics-forks/gazebo_ros_pkgs.git
 eband_local_planner             git melodic-devel             02252d0fecd1 github.com/pal-robotics-forks/eband_local_planner.git
 dynamic_introspection           git kinetic-devel             131d9100fd41 github.com/pal-robotics/dynamic_introspection.git
 ddynamic_reconfigure_python     git master                    d1dc50162fb8 github.com/pal-robotics/ddynamic_reconfigure_python
 custom_end_effector             git master                    23da28c6736e github.com/pal-robotics/custom_end_effector.git
 backward_ros                    git kinetic-devel             0eaa66390226 github.com/pal-robotics/backward_ros.git
 aruco_ros                       git noetic-devel              823e1f997b58 github.com/pal-robotics/aruco_ros.git
argenos commented 1 year ago

The only difference was I changed some lines in your rosinstall file to use e.g. https://github.com/argenos/tiago_simulation.git instead of git@github.com:argenos/tiago_simulation.git because it gave me permission errors.

Could this be a problem?

I don't think so, that's just because I prefer git over ssh.

Do the errors about TFs disappear eventually? Sometimes it's just a matter of waiting for all the nodes to finish their initialization.

Also the costmaps in blue/purple are not shown.

Does this happen for all robots? Maybe you can share a screenshot of rviz. And just to double-check, would this be the only remaining issue (if sending velocity commands and TF errors are "solved")?

jrharbin-york commented 1 year ago

I have sent goal commands to the robots in 'map' frame, via XXX/move_base_simple/goal, and all robots moved successfully to them. They seem to be really slow especially on beginning to move, taking a few minutes to move to the other side of their starting room, but they get there eventually :)

The TF errors do disappear eventually by the time the "Ready" is printed on the console. I think it might just be a matter of the startup delays and some components not being ready?

Thanks for all your help. I will close this issue now, and open another r.e. the costmaps, with a screenshot etc.

argenos commented 1 year ago

No need to open another issue, I found the problem and I’ll upload the fixes a bit later today.