There is something wrong with the controller_manager(spawner), the transform computation or the gazebo plugins used?

Hey people, my name is Eliomar. I am trying to recreate an alternative smulation simulation with the magni_gazebo package using just the lidar and not the sonars.

It was relatively easy because all was done and the Rviz configuration was also easy. But I am having a problem now in a slam simulation with that gazebo model and TF tree.

I am performing different tests and collision detection is one of them. The description of the test is the following:

I am using manual control to draw a map, the gmapping and all the slam is working well, but then if I want to do a collisiton detection test, which is going against the wall, to see what happen. And then the problem appears, apparently the rviz is not detecting collisions in the robot, so when the robot has collided with the wall but its wheels keep moving the rviz des not maintain the robot attached to the map wall but instead it keeps moving in the same direction going far and far away from the odom frame (which also moves wih respect to the map frame). I attach an image of this problems for better description:

At the beginning I thought the problem was rviz, but then I took the original empty_world.launch files and did the same tests placing a wall in gazebo world and I got the same result. (even with sonars enabled)

Then I started research what was happening and I noticed in comparison with other simulations that the odom frame was having and interesting movement with respect to the map frame that other simulations do not have, and sometimes it seems that the odom frame is fixed (example in the image)

That in magni_gazebo simulation case it is also fixed in the map but when it goes to the wall it does not stop when it collides, it just keep moving in rviz.

Until now I don't know what is the problem but I suspect that it could be the TF calculation and hence the controller_manager/spawner. because when I kill that node, gaze stops sending the /joint_states topic to the robot state publisher and in general some other TF stop.

Do you know what could be the problem? maybe a calculation in /robot_state_publisher, /controller_spawner (with the magni_controller.yaml file), or even in the gazebo model?

I am a little stuck in this step, so I would appreciate any help on this.

Thank you in advance for your help.

Hey Eliomar, i think i suspect what you have a problem with, but just to confirm, can you also post:

What branch of magni_robot you are on
The exact command(s) that you are starting everything with
the result of rosnode list
what is your fixed frame in Rviz - under Global options in Rviz

Hey Janez. Thanks very much for taking the time to review my issue. To answer your questions in order:

What branch of magni_robot you are on: I am in noetic-devel branch, but using with ROS melodic ( as the repo does not have a proper melodic branch, as far as I know, but I think it works very well actually.
The exact command(s) that you are starting everything with: I have created my own launch file, based on the "empty_world.launch" file, so my command is:

roalsunch magni_gazebo empty_world_slam.launch which is a copy of empty_world launch just with the difference that I am disabling the sonars, loading another rviz config (for slam) and also launching the gmapping node. All the other calls to "magni.launch", "description.launch" (in magni_description) are the same.

the result of rosnode list: (attached picture too)

$ rosnode list  
/cmd_vel_relay  
/controller_spawner  
/gazebo  
/gazebo_gui  
/magni_rqt_teleop  
/magni_slam_gmapping  
/odom_relay  
/robot_state_publisher  
/rosout  
/rviz_eliomar_inspiron_5590_16877_2322677021695778765

what is your fixed frame in Rviz - under Global options in Rviz: (attached picture) In this slam case is the map, but in the initial post in the other examples is also odom (pictures 2 to 4). both with the same problem.

Again. Thanks in advance for your help Janez.

but then if I want to do a collisiton detection test, which is going against the wall, to see what happen. And then the problem appears, apparently the rviz is not detecting collisions in the robot, so when the robot has collided with the wall but its wheels keep moving the rviz des not maintain the robot attached to the map wall but instead it keeps moving in the same direction going far and far away from the odom frame

Yes, so running pure empty_world.launch will spawn the robot in empty world and you'll be able to drive around manually but it does not run "collision detection". This means that robot will hit the wall if you command it to do so through cmd_vel and continue spinning the wheels in place. Because one of the inputs of your slam node is also wheel odometry, the robot will think its moving forward where as in simulation is standing still. This results in robots map to jump arround unpredictably like you see in Rviz.

We provide the "collision detection" within our move_basic which only works for robot following goals (and not your cmd_vel manual driving).

Ahmmmm I understand. But if it is in the move_basic package, how can I add it to my simulation?

Well, I think I will check that move basic package. But I mean, that thing of the wheels still spinning, while the robot just collided against the wall is a TF problem, right? Because I just see how the base_footprint frame moves with respect to the odom frame while the wheels are spining (even against the wall).

yes, robot has collided with the wall and it cant go forward. Its peferctly normal that the wheels still spin forward if you manually command it so - but the wheels are slipping on the ground. And because of how wheel odometry is designed (calculated only from the encoder ticks) the slam algorithm actually thinks the robot is still moving forward, where as in reality it is not. This causes the missalignments. This is standard problem in SLAM and wheel odometry. You solve it with collision checkers taking care that the robot does not hit objects and cause wheel slippage.

Ahmmmm I understand. But if it is in the move_basic package, how can I add it to my simulation?

Try launching and experimenting with the https://github.com/UbiquityRobotics/magni_robot/blob/noetic-devel/magni_nav/launch/move_basic.launch. How move baisc actually works can be seen on the repo README

oh! perfect then Janez. I will check that and will be back if I have any other doubt. Thank you a lot.

Hey @JanezCim . Thanks a lot for your help.

I was checking the move_basic.launch. And effectively it runs a collision avoidance and it avoids the robot to hit objects. And according to your explanation, yeah, it is quite logic that the robot keeps moving in rviz if the wheels are slipping in the ground. But, what if it is not so easy to avoid collisions? there is a proper collision detection system in Magni?

Besides that I was testing a navigation environment that I set. and it is working properly (more or less) with some amcl parameters for magni. But when I run some navigation goal I am observing that the robot loses its aligned with the map, so after reaching a goal I get something like this:

Maybe because the wheels slipped at some point? I also noted that it tends to misalign with rotations at a higher speed, so for that reason I ask if there is possible that the robot slips doing that?

Besides that, it is possible to create a more robust wheel odometry calculation? one that could take into account some slips? or maybe something that detects the collision effectively even with the wheels slipping? I am thiking more in manual control and teleoperation, where the user can effectively have some collisions, or when the robot loses its alignment with the map during navigation.

But, what if it is not so easy to avoid collisions? there is a proper collision detection system in Magni?

move basic that was developed inhouse is ment as a very simple version of ros-standard move_base. move_basic can only do crude stop on obstacle (no avoidance, no fancy clearing mechanisms) because its ment to be as simple as possible. But if you need something more advanced, i'd recommend move_base, which we have worked with in the past, and there is example configuration for in https://github.com/UbiquityRobotics/magni_robot/blob/noetic-devel/magni_nav/launch/move_base.launch but has MANY more parameters to tune - take our config more as a guide, not as a complete setup (it was updated 5 years ago).

Maybe because the wheels slipped at some point? I also noted that it tends to misalign with rotations at a higher speed, so for that reason I ask if there is possible that the robot slips doing that?

many possible reasons. Also wrong amcl params, uncorrect sensor positioning, ... See amcl tuning guide for more info. I personally am more a fan of https://github.com/iris-ua/iris_lama_ros if you need an alternative

Besides that, it is possible to create a more robust wheel odometry calculation? one that could take into account some slips?

Theoretically yes up to a certain degree, but it is currently not implemented is the short answer. Wheel odometry is going to have a small drift no matter what you do. That is why you do sensor fusion and slam. Btw you can hire Ubiquity to do custom projects for you (hardware and/or software), for that contact them on https://www.ubiquityrobotics.com/contact/

anyway we are slowly drifting away from the original question. Please close the issue if you feel like your original question was answered and consider opening a new one if you have other problems

Yeah. Definitely I got my question answered. thank you a lot Janez.

Actually @JanezCim , Sorry to reopen the post, but as my question was also regarding the TF computation I have a last question before closing the post definitely.

I was following this guide to tune the navigation stack as you suggested. http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide

And in the odometry part I was doing the rotation tests, to see if the odometry was being calulated well.

I obtained that for rotation the lasers differ a little and are not exactly one on top of other. but my question is: it is possible that this is one of the problems for the map misalignment? it's because I don't know much how to measure the quality of the odometry computation instead of doing this.

I think in my opinion this lasers match pretty well (in the attached image) while the robot is rotating and the fixed frame is "odom".

Maybe you can also confirm me that everything is ok in this case with the odometry calculation?

But I also wanted to ask if this is ok? (the images below) I mean it seems logic to have some drift in the scans if I set the decay time in 20 for example, and it also seems logic that the delay drawn in rviz has different patterns depending on the map right? Like this:

In contrast to those 2 images that look pretty well, there is other image that makes me worried about the odometry, according to this navigation tuning guide http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide.

it should not exist much rotational drift as in this image:

So, maybe there is a problem in odometry? or it depends on the map that is runnig in gazebo? Reminding that I am not running anything related to navigation stack or slam, just the robot_state publisher for the TF, gazebo, and rviz. (attached pictures for nodes and topics). Maybe I am missing some especial documentation as this is a 270° LiDAR? Well, it's the original lidar in magni_gazebo package

I also performed a comparison with another simulation and the same map that seems to perform very well when observing those rotational drifts.

I think in my opinion this lasers match pretty well (in the attached image) while the robot is rotating and the fixed frame is "odom".

if your fixed frame in rviz is "odom" then the lidar scan "missalignment" you see might also be caused by odometry drifting since odometry outputs tf odom -> base_footprint. Great resources to read how tfs are calculated on rep105 and https://linklab-uva.github.io/autonomousracing/assets/files/L11-compressed.pdf

I mean it seems logic to have some drift in the scans if I set the decay time in 20 for example, and it also seems logic that the delay drawn in rviz has different patterns depending on the map right?

To mee this seems logical since if your fixed frame is "odom" then what youre seeing is slight odometry drift.

In contrast to those 2 images that look pretty well, there is other image that makes me worried about the odometry, according to this navigation tuning guide http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide.

Yes this is wierd to me. Can you please post what parameters you've changed after following the tunning guide?

For the first two images that I posted it seems like a normal behavior. But the third one was very strange to see all that odometry drift.

And actually I did not change anything. In navigation I used some costmap parameters, but these tests that I am doing are without any slam or navigation package. it's just loading the gazebo model inside the map.

The only things that I am changing are the map ad the initial position of the robot in that map, basically I am just editing "empty_world.launch" for the map and "magni.launch" for the position in the map. It can be reproducible if you want. The last test that I performed I even included the sonars but nothing improved, and the fixed frame is odom. I get this:

Regarding the parameters that I changed after following the tuning guide, that's exactly the point. I don't know what to change haha =/. I don't want to mess with the gazebo model, as it should be perfect. So my question is, maybe you can point me what parameters do I need to change? Because it seems that the problem is the odometry source, and that comes from gazebo, well, exactly from the controller_spawner node, for that reason I asked if that was working well. =/

Once again, thanks for all your help Janez.

The only things that I am changing are the map ad the initial position of the robot in that map, basically I am just editing "empty_world.launch" for the map and "magni.launch" for the position in the map. It can be reproducible if you want. The last test that I performed I even included the sonars but nothing improved, and the fixed frame is odom.

I dont understand what exactly you mean here. It would be best if you copy-paste the relevant launches/parameters that you've changed when the problem occoured so i can try to replicate this and try to make sense of it. I think after we solve this, other stuff will make sense too.

ok ok sure. I am changing the empty_world.launch to have this:

<launch>

    <!-- 
    This string will be taken to search for camera extrinsics yaml file like 
    $(find magni_description)/extrinsics/camera_extrinsics_$(arg camera_position).yaml
    so if that yaml file does not exsist, the xacro file will return "No such file or directory" error
    -->
    <arg name="camera_position" default="forward"/> 

    <!-- 
    This string will be taken to search for lidar extrinsics yaml file like 
    $(find magni_description)/extrinsics/lidar_extrinsics_$(arg lidar_position).yaml
    so if that yaml file does not exsist, the xacro file will return "No such file or directory" error
    -->
    <arg name="lidar_position" default="top_plate"/>

    <arg name="sonars_installed" default="true"/>

    <arg name="rviz_config" default="true" />

    <!-- load empty world -->
    <arg name="gui" default="true"/>
    <arg name="headless" default="false" />
    <arg name="debug" default="false" />
    <arg name="paused" default="false"/>
    <arg name="use_sim_time" default="true"/>

    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_house.world"/>
        <arg name="gui" value="$(arg gui)" />
        <arg name="headless" value="$(arg headless)" />
        <arg name="paused" value="$(arg paused)" />
        <arg name="debug" value="$(arg debug)" />
        <arg name="use_sim_time" value="true" />
    </include>

    <!-- start robot -->
    <!-- Initial position of the robot -->
    <arg name="x_pos" default="-3.0"/>
    <arg name="y_pos" default="1.0"/>
    <arg name="z_pos" default="0.0"/>

    <!-- start robot -->
    <include file="$(find magni_gazebo)/launch/magni.launch">
        <arg name="camera_position" value="$(arg camera_position)"/>
        <arg name="sonars_installed" value="$(arg sonars_installed)"/>
        <arg name="lidar_position" value="$(arg lidar_position)"/>
        <arg name="x_pos" value="$(arg x_pos)"/>
        <arg name="y_pos" value="$(arg y_pos)"/>
        <arg name="z_pos" value="$(arg z_pos)"/>
    </include>

    <!-- Additional nodes (optional)  -->

    <!-- RViz to visualize robot state -->
    <arg unless="$(arg rviz_config)" name="rviz_args" value="" />
    <arg     if="$(arg rviz_config)" name="rviz_args" value="-d $(find magni_gazebo)/launch/rviz_config.rviz" />

    <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false" args="$(arg rviz_args)" output="screen"/>

    <!-- GUI for robot control via 'cmd_vel' topic -->
    <node pkg="rqt_robot_steering" type="rqt_robot_steering" name="magni_rqt_teleop">
        <param name="default_topic" type="str" value="/ubiquity_velocity_controller/cmd_vel"/>
        <param name="default_vx_max" type="double" value="0.8" />
        <param name="default_vx_min" type="double" value="-0.8" />
        <param name="default_vw_max" type="double" value="1.5" />
        <param name="default_vw_min" type="double" value="-1.5" />
    </node>

</launch>

I am using the turtlebot3_house.world to save time in the building of a custom map.

and I am modifying then after that the magni.launch file

<?xml version="1.0" encoding="UTF-8"?>
<launch>

    <!-- 
    This string will be taken to search for camera extrinsics yaml file like 
    $(find magni_description)/extrinsics/camera_extrinsics_$(arg camera_position).yaml
    so if that yaml file does not exsist, the xacro file will return "No such file or directory" error
    Leave empty to disable
    -->
    <arg name="camera_position" default="forward"/>

    <!-- 
    This string will be taken to search for lidar extrinsics yaml file like 
    $(find magni_description)/extrinsics/lidar_extrinsics_$(arg lidar_position).yaml
    so if that yaml file does not exsist, the xacro file will return "No such file or directory" error
    Leave empty to disable
    -->
    <arg name="lidar_position" default="top_plate"/>

    <arg name="sonars_installed" default="true"/>

    <arg name="lidar_extrinsics_file" value="$(find magni_description)/extrinsics/lidar_extrinsics_$(arg lidar_position).yaml"/>
    <arg name="camera_extrinsics_file" value="$(find magni_description)/extrinsics/camera_extrinsics_$(arg camera_position).yaml"/>

    <include file="$(find magni_description)/launch/description.launch">
        <arg name="camera_extrinsics_file" value="$(arg camera_extrinsics_file)" />
        <arg name="sonars_installed" value="$(arg sonars_installed)" />
        <arg name="lidar_extrinsics_file" value="$(arg lidar_extrinsics_file)" />
    </include>

    <arg name="x_pos" default="-3.0"/>
    <arg name="y_pos" default="1.0"/>
    <arg name="z_pos" default="0.0"/>

    <node name="magni_spawn" pkg="gazebo_ros" type="spawn_model" output="screen" args="-urdf -param robot_description -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -model magni" />

   <!-- Load the parameters used by the following nodes -->
    <rosparam file="$(find magni_gazebo)/config/magni_controllers.yaml" command="load"/>
    <!-- Launch the roscontrol controllers needed -->
    <node name="controller_spawner" pkg="controller_manager" type="spawner" args="ubiquity_velocity_controller ubiquity_joint_publisher"/>

    <!-- Topic redirection for compatibility with real robot configuration (core.launch) -->
    <!-- 'topic_tools relay' is used to redirect topics, because remap doesn't work in Gazebo. -->
    <node name="cmd_vel_relay" type="relay" pkg="topic_tools" args="/cmd_vel /ubiquity_velocity_controller/cmd_vel" />
    <node name="odom_relay" type="relay" pkg="topic_tools" args="/ubiquity_velocity_controller/odom /odom" />

    <node name="sonar0_relay" type="relay" pkg="topic_tools" args="/pi_sonar/sonar_0 /sonars" />
    <node name="sonar1_relay" type="relay" pkg="topic_tools" args="/pi_sonar/sonar_1 /sonars" />
    <node name="sonar2_relay" type="relay" pkg="topic_tools" args="/pi_sonar/sonar_2 /sonars" />
    <node name="sonar3_relay" type="relay" pkg="topic_tools" args="/pi_sonar/sonar_3 /sonars" />
    <node name="sonar4_relay" type="relay" pkg="topic_tools" args="/pi_sonar/sonar_4 /sonars" />

</launch>

Then I just do roslaunch magni_gazebo empty_world.launch

So if you see the line 41 in magni.launch file, we have the controller_manager package launching a spawner. but the curious if that if you check that node with rosnode info you will see this:

eliomar@eliomar-inspiron-5590:~$ rosnode info /controller_spawner
--------------------------------------------------------------------------------
Node [/controller_spawner]
Publications: 
 * /rosout [rosgraph_msgs/Log]

Subscriptions: 
 * /clock [rosgraph_msgs/Clock]

Services: 
 * /controller_spawner/get_loggers
 * /controller_spawner/set_logger_level

contacting node http://eliomar-inspiron-5590:44789/ ...
Pid: 20257
Connections:
 * topic: /rosout
    * to: /rosout
    * direction: outbound (45495 - 127.0.0.1:60834) [9]
    * transport: TCPROS
 * topic: /clock
    * to: /gazebo (http://eliomar-inspiron-5590:37923/)
    * direction: inbound
    * transport: TCPROS

apparently no important topics coming from it. but if I you kill that Node. Gazebo stops sending the odometry source. So for that I was wondering if that node is working well, as it is external to the magni model.

Sorry for a bit of delay. I've tested this and was able to reproduice your problem. When i've copy-pasted your launch folders, launched with roslaunch magni_gazebo empty_world.launch and set the robot to rotate i've got the scan to rotate to an unreasonably large degree (notice the static frame odom and lidar decay of 5 seconds): lidarg1

But then i've commented out the  in the empty_world.launch and after running roslaunch magni_gazebo empty_world.launch ive inserted the same house manually from the Insert Gazebo menu. All other stuff was the same as in first experiment. I've experienced MUCH less drift, sometimes almost zero:

Can you reproduce this experiment and message if youre seeing the same?

Hey Janez. Yeah sure I can do that and come back with the results.

Hey Again Janez. I did reproduce the test that you told me but I had more or less the same result with odom as fixed frame and decay time 5.

But I have to say that after checking and cking some guides I managed to solve it modifying the gazebo model of the robot and using the differential drive provided by the gazebo libraries here and deleting the xacro loading of the transmission file in the magni.urdf.xacro file. So these are my files:

magni.urdf.xacro (also I added an IMU but I am not using it):

<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="magni">

  <xacro:arg name="sonars_installed" default="true"/>
  <xacro:property name="sonars_installed" value="$(arg sonars_installed)"/>

  <xacro:arg name="camera_extrinsics_file" default=""/>
  <xacro:property name="camera_extrinsics_file" value="$(arg camera_extrinsics_file)"/>

  <xacro:arg name="lidar_extrinsics_file" default=""/>
  <xacro:property name="lidar_extrinsics_file" value="$(arg lidar_extrinsics_file)"/>

  <xacro:property name="wheel_r" value="0.1" />
  <xacro:property name="wheel_xpos" value="0.11" />

  <xacro:include filename="$(find done_magni_simulation)/urdf/magni_modified.gazebo.xacro" />
  <xacro:include filename="$(find magni_description)/urdf/inertial.xacro" />
  <xacro:include filename="$(find magni_description)/urdf/sensors/sonar_hc-sr04.xacro" />
  <xacro:include filename="$(find magni_description)/urdf/sensors/raspi_camera.xacro" />
  <xacro:include filename="$(find magni_description)/urdf/sensors/ur50_lidar.xacro" />

  <!-- Robot base_link -->
  <link name="base_link">
    <visual>
      <origin xyz="${-wheel_xpos} 0 ${-wheel_r+0.071}" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://magni_description/meshes/magni_body.dae" />
      </geometry>
    </visual>
    <visual>
      <origin xyz="${-wheel_xpos-0.169} 0.17 ${-wheel_r+0.09}" rpy="0 0 ${pi}" />
      <geometry>
        <mesh filename="package://magni_description/meshes/caster_wheel.dae" />
      </geometry>
    </visual>
    <visual>
      <origin xyz="${-wheel_xpos-0.169} -0.17 ${-wheel_r+0.09}" rpy="0 0 ${pi}" />
      <geometry>
        <mesh filename="package://magni_description/meshes/caster_wheel.dae" />
      </geometry>
    </visual>
    <collision>
      <origin xyz="${-wheel_xpos} 0 ${-wheel_r+0.151}" rpy="0 0 0" />
      <geometry>
        <box size="0.398 0.268 0.160"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="${-wheel_xpos-0.026} 0 ${-wheel_r+0.248}" rpy="0 0 0" />
      <geometry>
        <box size="0.375 0.268 0.034"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="${-wheel_xpos-0.169} 0.169 ${-wheel_r+0.106}" rpy="0 0 0" />
      <geometry>
        <box size="0.1 0.07 0.07"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="${-wheel_xpos-0.169} -0.169 ${-wheel_r+0.106}" rpy="0 0 0" />
      <geometry>
        <box size="0.1 0.07 0.07"/>
      </geometry>
    </collision>
    <xacro:box_inertial_with_origin  x="0.398" y="0.221" z="0.150" mass="10.0">
      <origin xyz="-0.15 0 0.03" rpy="0 0 0" />
    </xacro:box_inertial_with_origin>
  </link>

  <!-- Fixed offset from the floor to the center of the wheels -->
  <link name="base_footprint"/>
  <joint name="base_footprint_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0 0 ${wheel_r}" rpy="0 0 0"/>
  </joint>

  <!-- Macro for defining wheels -->
  <xacro:macro name="wheel" params="prefix reflect">
    <link name="${prefix}_wheel">
      <visual>
        <origin xyz="${-0.071*reflect} 0 0" rpy="0 ${(reflect-1)*pi/2} 0" />
        <geometry>
          <mesh filename="package://magni_description/meshes/front_wheel.dae" />
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 ${pi/2} 0" />
        <geometry>
          <cylinder radius="${wheel_r}" length="0.05"/>
        </geometry>
      </collision>
      <collision>
        <origin xyz="${0.033*reflect} 0 0" rpy="0 ${pi/2} 0" />
        <geometry>
          <cylinder radius="0.064" length="0.016"/>
        </geometry>
      </collision>
      <xacro:cylinder_inertial_with_origin radius="${wheel_r}" length="0.05" mass="3.34">
        <origin xyz="0 0 0" rpy="0 ${pi/2} 0" />
      </xacro:cylinder_inertial_with_origin>
    </link>
    <joint name="${prefix}_wheel_joint" type="continuous">
      <axis xyz="1 0 0" rpy="0 0 0" />
      <parent link="base_link"/>
      <child link="${prefix}_wheel"/>
      <origin xyz="0 ${0.163*reflect} 0" rpy="0 0 ${pi/2}"/>
    </joint>
  </xacro:macro>

  <xacro:wheel prefix="left" reflect="1"/>
  <xacro:wheel prefix="right" reflect="-1"/>

  <!-- Macro for defining caster wheels -->
  <xacro:macro name="caster_wheel" params="prefix reflect">
    <link name="${prefix}_caster_wheel">
      <collision>
        <origin xyz="${-wheel_xpos-0.19} ${reflect*0.169} ${-wheel_r+0.04}" rpy="0 0 0" />
        <geometry>
          <sphere radius="0.04"/>
        </geometry>
      </collision>
    </link>
    <joint name="${prefix}_caster_wheel_joint" type="fixed">
      <parent link="base_link"/>
      <child link="${prefix}_caster_wheel"/>
    </joint>
  </xacro:macro>

  <xacro:caster_wheel prefix="left" reflect="1"/>
  <xacro:caster_wheel prefix="right" reflect="-1"/>

  <!-- This is a bonus IMU, take care a lot of this -->
  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin xyz="-0.032 0 0.068" rpy="0 0 0"/>
  </joint>

  <link name="imu_link"/>

  <!-- Define all sonars, if sonars_installed set to 1 -->
  <xacro:if value="${sonars_installed}">
    <xacro:sonar_hc-sr04 name="sonar_0" connected_to="base_link">
      <origin xyz="0.01 -0.14 0.15" rpy="0 0 ${-pi/2}"/>
    </xacro:sonar_hc-sr04>
    <xacro:sonar_hc-sr04 name="sonar_1" connected_to="base_link">
      <origin xyz="0.08 -0.07 0.15" rpy="0 0 0.785"/>
    </xacro:sonar_hc-sr04>
    <xacro:sonar_hc-sr04 name="sonar_2" connected_to="base_link">
      <origin xyz="0.08 -0.01 0.15" rpy="0 0 -0.785"/>
    </xacro:sonar_hc-sr04>
    <xacro:sonar_hc-sr04 name="sonar_3" connected_to="base_link">
      <origin xyz="0.08 0.04 0.15" rpy="0 0 0"/>
    </xacro:sonar_hc-sr04>
    <xacro:sonar_hc-sr04 name="sonar_4" connected_to="base_link">
      <origin xyz="0.01 0.14 0.15" rpy="0 0 ${pi/2}"/>
    </xacro:sonar_hc-sr04>
  </xacro:if>

  <!-- Define camera -->
  <xacro:if value="${camera_extrinsics_file != ''}">
    <xacro:property name="cam_extrinsics" value="${load_yaml(camera_extrinsics_file)}"/>
    <xacro:raspi_camera name="raspicam" connected_to="base_link">
      <origin xyz="${cam_extrinsics['x']} ${cam_extrinsics['y']} ${cam_extrinsics['z']}"
        rpy="${cam_extrinsics['roll']} ${cam_extrinsics['pitch']} ${cam_extrinsics['yaw']}"/>
    </xacro:raspi_camera>
  </xacro:if>

  <!-- Define Lidar -->
  <xacro:if value="${lidar_extrinsics_file != ''}">
    <xacro:property name="lidar_extrinsics" value="${load_yaml(lidar_extrinsics_file)}"/>
    <xacro:ur50_lidar name="laser" connected_to="base_link">
      <origin xyz="${lidar_extrinsics['x']} ${lidar_extrinsics['y']} ${lidar_extrinsics['z']}"
        rpy="${lidar_extrinsics['roll']} ${lidar_extrinsics['pitch']} ${lidar_extrinsics['yaw']}"/>
    </xacro:ur50_lidar>
  </xacro:if>

</robot>

magni.gazebo.xacro:

<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:arg name="imu_visual"   default="false"/>

    <gazebo>
        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
            <legacyModeNS>true</legacyModeNS>
        </plugin>
    </gazebo>

    <gazebo>
        <plugin name="gazebo_ros_p3d" filename="libgazebo_ros_p3d.so">
            <updateRate>100</updateRate>
            <bodyName>base_footprint</bodyName>
            <topicName>exact_pose</topicName>
            <frameName>map</frameName>
        </plugin>
    </gazebo>

    <gazebo reference="left_wheel">
        <mu1>1.0</mu1>
        <mu2>1.0</mu2>
        <fdir1>0 0 1</fdir1>
        <minDepth>0.005</minDepth>
        <kp>1e8</kp>
    </gazebo>
    <gazebo reference="right_wheel">
        <mu1>1.0</mu1>
        <mu2>1.0</mu2>
        <fdir1>0 0 1</fdir1>
        <minDepth>0.005</minDepth>
        <kp>1e8</kp>
    </gazebo>

    <gazebo reference="left_caster_wheel">
        <mu1>0.0</mu1>
        <mu2>0.0</mu2>
        <minDepth>0.005</minDepth>
        <kp>1e8</kp>
    </gazebo>
    <gazebo reference="right_caster_wheel">
        <mu1>0.0</mu1>
        <mu2>0.0</mu2>
        <minDepth>0.005</minDepth>
        <kp>1e8</kp>
    </gazebo>

    <gazebo>
        <plugin name="magni_controller" filename="libgazebo_ros_diff_drive.so">
              <commandTopic>cmd_vel</commandTopic>
              <odometryTopic>odom</odometryTopic>
              <odometryFrame>odom</odometryFrame>
              <odometrySource>world</odometrySource>
              <publishOdomTF>true</publishOdomTF>
              <robotBaseFrame>base_footprint</robotBaseFrame>
              <publishWheelTF>false</publishWheelTF>
              <publishTf>true</publishTf>
              <publishWheelJointState>true</publishWheelJointState>
              <legacyMode>false</legacyMode>
              <updateRate>30</updateRate>
              <leftJoint>left_wheel_joint</leftJoint>
              <rightJoint>right_wheel_joint</rightJoint>
              <wheelSeparation>0.326</wheelSeparation>
              <wheelDiameter>0.2</wheelDiameter>
              <wheelAcceleration>1</wheelAcceleration>
              <wheelTorque>10</wheelTorque>
              <rosDebugLevel>na</rosDebugLevel>
        </plugin>
    </gazebo>

    <gazebo reference="imu_link">
        <sensor type="imu" name="imu">
          <always_on>true</always_on>
          <visualize>$(arg imu_visual)</visualize>
        </sensor>
        <material>Gazebo/FlatBlack</material>
    </gazebo>

    <gazebo>
      <plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
        <alwaysOn>true</alwaysOn>
        <bodyName>imu_link</bodyName>
        <frameName>imu_link</frameName>
        <topicName>imu</topicName>
        <serviceName>imu_service</serviceName>
        <gaussianNoise>0.0</gaussianNoise>
        <updateRate>0</updateRate>
        <imu>
          <noise>
            <type>gaussian</type>
            <rate>
              <mean>0.0</mean>
              <stddev>2e-4</stddev>
              <bias_mean>0.0000075</bias_mean>
              <bias_stddev>0.0000008</bias_stddev>
            </rate>
            <accel>
              <mean>0.0</mean>
              <stddev>1.7e-2</stddev>
              <bias_mean>0.1</bias_mean>
              <bias_stddev>0.001</bias_stddev>
            </accel>
          </noise>
        </imu>
      </plugin>
    </gazebo>

</robot>

I have to say that this also solved the problem of collision detection in gazebo. So if the robot collisions against a wall the rviz won't show the robot still moving from the odometry source, even if the wheels are sliping.

here a demosntration( for the sanity check of the odometry source):

and for the collision detection:

I don't know if this is ok. But this solve my problem. Maybe you can give me a better advice on how to solve it?

Thanks again for your help.

Sorry for a big delay again.

When trying to reproduce i got error

$ roslaunch magni_gazebo empty_world.launch
... logging to /home/janez/.ros/log/4b480b42-c471-11ec-bb1c-1b26d45ff4d9/roslaunch-zenbook-10353.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

xacro: in-order processing became default in ROS Melodic. You can drop the option.
resource not found: done_magni_simulation
ROS path [0]=/opt/ros/noetic/share/ros
ROS path [1]=/home/janez/Documents/Projects/ubiquityrobotics/magnii_ws/src
ROS path [2]=/home/janez/iris_lama_ws/src/iris_ur_lama
ROS path [3]=/home/janez/iris_lama_ws/src/iris_ur_lama_ros
ROS path [4]=/opt/ros/noetic/share
when processing file: /home/janez/Documents/Projects/ubiquityrobotics/magnii_ws/src/magni_robot/magni_description/urdf/magni.urdf.xacro

so you'd need to add the done_magni_simulation

BUT anyway to me your comment:

I have to say that this also solved the problem of collision detection in gazebo. So if the robot collisions against a wall the rviz won't show the robot still moving from the odometry source, even if the wheels are sliping.

indicates that your additions are not working correctly, since odometry should get lost if the wheels are slipping after the robot has colided with the wall, so I think your solution is not quite right. Yet.

In my opinion, we should be looking at improving the existing odom, not completely changing it. But this involves a deep dive into code, which we currently dont have a lot of time for on our end. Will get to this eventually though.

ahmmm yeah I think I made a mistake and did not upload all the files. ehhh done_magni_simulation is the package that I created to test all the modifications that I did.

But then it is a bug? I think yeah my solution is not quite right, but it improves the odometry source. Do you have a time estimation to fix this bug?

But then it is a bug?

It has a potential for a bug and it should be researched.

Do you have a time estimation to fix this bug?

Currently no because theres a lot of priority items on our end that i dont know the eta for, sorry.

Hey Janez. ok ok. No problem then I think for the moment the modifications that I made are enough for the autonomous algorithms that I am testing. The problem was that with the old odometry was very hard to use the localization node.

Ok having read through this entire saga, this gist of it is that the Magni's angular odometry drifts more than expected in Gazebo?

I mean that's not even a bug, that's a feature. The real robot is arguably just as bad when it comes to rotational odometry, it can be up to a few degrees off in a 180 deg rotation maneuver. Just means the covariances are set up correctly in the simulated controller.

Closing this for now unless there's anything else. Or actually, if there is anything else I would suggest opening a new issue, this one is already long enough.

UbiquityRobotics / magni_robot

There is something wrong with the controller_manager(spawner), the transform computation or the gazebo plugins used? #202