Adding reset model plugin for gazebo sim

[shrit]

Signed-off-by: Omar Shrit shrit@lri.fr

[julianoes]

Interesting. How do you trigger this?

[shrit]

@julianoes I have only to call this function, and it will reset all the quadrotors models

void Gazebo::reset_models()
{
  for (auto it : pubs_) {
    if (it->WaitForConnection(5)) {
      gazebo::msgs::Vector2d msg;
      gazebo::msgs::Set(&msg, ignition::math::Vector2d(1, 0));
      it->Publish(msg);
    } else {
      LogDebug() << "NO Connection from the subscriber to reset the model";
    }
  }
}

[julianoes]

So a Vector2d(1, 0) means reset? Shouldn't we have a custom message for that?

[shrit]

Well, I think we should, since I did not find any message, and I have to find a solution quickly, I ended up using only one part of a vector2D, which is not appropriate.
We need to define a reset message and use it in this case.

[Jaeyoung-Lim]

@shrit As far as I can see, this will only reset the simulation state of the model and not the firmware.

Wouldn't this result in bad local position estimation? Do you have any logs of the firmware to see what happens when you reset the model?

[shrit]

Well, if you wait for around 20 seconds after the resetting, you will have a very low chance of getting bad local position estimation.
Though resetting the firmware need to be implemented in the firmware itself (not implemented yet), then if you would like to reboot the firmware, this can be realized by using the reboot command in the MAVSDK.

[shrit]

@Jaeyoung-Lim For the log, actually I do not have any firmware log, since I verify that all the quadrotors are landed before resetting them.

[Jaeyoung-Lim]

@shrit For me this plugin needs to include the firmware reboot since resetting just the position in simulation is a little wierd for me. I believe in some cases the EKF might not be able to recover.

The reboot does not necessarily need to be done from mavsdk. You can just send the reboot command via mavlink through the gazebo_mavlink_interface

[TSC21]

There is no need to make this a plugin. The functionality is so simple that can be brought into a function in common.h and be called in gazebo_mavlink_interface.

[shrit]

Just to summerise everything:
@Jaeyoung-Lim I totally agree with you, I even have to increase the COM_ARM_EFK_AB in order to make EKF recover more easily. It would be much better to reboot PX4 at the same time with the resetting. However, Do you have any starting point?
@@TSC21 As there is no need to make this as a plugin. Would you think it is a good idea to reset PX4 with the physical model in the simulator by implementing everything in common.h ?

[TSC21]

Just to summerise everything:
@Jaeyoung-Lim I totally agree with you, I even have to increase the COM_ARM_EFK_AB in order to make EKF recover more easily. It would be much better to reboot PX4 at the same time with the resetting. However, Do you have any starting point?
@@TSC21 As there is no need to make this as a plugin. Would you think it is a good idea to reset PX4 with the physical model in the simulator by implementing everything in common.h ?

Yes. It's as simple as sending a command. No complexity around it, compared with other plugins we have here.

[shrit]

Trying to merge reset and reboot the px4 firmware, I have discovered that the reboot command is killing the entire simulation session as doing shutdown

[TSC21]

@shrit any updates on this? Again, I don't think that a plugin specific for this is required. Just a note that HIL_SENSOR fields_updated bitmask as a bit field 31 that if set informs the SITL side that full reset of attitude/position/velocities/etc was performed in sim. We do not have yet support for that bit field parsing on PX4 but that could be a plausible way of doing this.

[shrit]

@TSC21 I agree with you, we do not have to create a separate plugin, but I did not work on it since we have to reboot the firmware with the physical state of the quadrotor instead of only resetting the quadrotors. This will avoid any potential problems with EKF. At this time, using the reboot command in the SDK to reboot the firmware will only shutdown the simulation since there is no real reboot for PX4 in SITL. I have opened an issue about this problem which can be found here: PX4/PX4-Autopilot#14070, there is an open pull request by @julianoes which can be found here: PX4/PX4-Autopilot#11654
@TSC21 Please tell me what do you think? Do we keep this open until we have a true reboot for PX4 inside the simulator? Otherwise is there any way to reset only EKF when resetting the position instead of the entire PX4

[julianoes]

PX4/PX4-Autopilot#11654 will need a lot more work.

[Jaeyoung-Lim]

@shrit Is there anything being done regarding this PR?

[shrit]

@Jaeyoung-Lim I can not tell, since it depends on several issues that they went on stale already

[Jaeyoung-Lim]

@shrit My question was if there was any updates on your side, since you opened this PR.

[shrit]

@Jaeyoung-Lim I think I have developed more on this since the last commit, but finally, we need to merge this functionality directly into common.h

[Jaeyoung-Lim]

@shrit This PR seems to be stale for almost a year. I think functionality is something that we do need, but since there is no activity, I am closing this PR. Please re open if you get the time to work on this agian

[shrit]

No worries, I am very occupied at this time, not even before next year, so you can close it without any issue.