Unable to reinitialize last saved pose after a system HARD reset on SIM1000

[koyalbhartia]

I am trying to use the service SickLocAutoStartSavePose to reinitialize the system by itself on restart (Hardware restart) at the pose where the telegram has been last called.

Following are the steps that I am follow to achieve the same:

1. Enable start of localization using the service: SickLocSetAutoStartActive (request.active = true)
2. Verify the above using the service: SickLocSetAutoStartActive (response.active = true)
3. Save the current pose using the service: SickLocAutoStartSavePose
4. Also tried doing Step 3 using the service: SickLocAutoStartSavePoseInterval

However, when I test the above in the below 2 scenarios:
Software reset: The last pose is reinitialized even without having to use the above functionality of AutoStartSavePose.
Hardware reset: The system reinitializes itself to the origin (0,0) of the map. :(

Hence I fail to see how I can use the service to implement an automatic reinitialization of pose after a system reset.
Also can I check where the pose is getting saved by the service in Step 3.
I am sure there should be a solution to this, and hopefully I am wrong somewhere with my above steps.
Thanks.

[rostest]

Thanks for your feedback! Just to be on the safe side: Service SickLocAutoStartSavePose answers the request with success: True? After a hardware reset and restart, the SIM runs as expected (except the initial pose) with green status LEDs and continues to send result port telegrams?

Please make sure the latest driver and firmware version is installed. The services LocSetAutoStartActive and LocAutoStartSavePose have been introduced in august 2020 and require LLS-1.3.0 or later. The best way to check is use of SOPASair.

If LocAutoStart is enabled, the system will reinitialize itself on restart at the pose where the telegram has been last called. Ensure that the vehicle is standing still, when the telegram is sent. Calling the telegram while the vehicle is still in movement, can lead to wrong initializations.

[koyalbhartia]

1. Yes, The service SickLocAutoStartSavePose answers with success: True
2. Yes, the SIM runs as expected after the reset with LEDs as well as telegram messages.
3. Yes, Our version is LL3-1.4 as shown in the attachment.
4. Yes, I followed the steps as shown in the first message to use the service and the vehicle was not moving.

Request any example of service usage or any other way to debug the situation or achieve the functionality.
Thanks.

[rostest]

Thanks for your reply! To narrow down the fault, please retry the LocSetAutoStartActive command using Cola-Telegrams directly:

rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocAutoStartSavePose', wait_response_timeout: 1}"

Can you post the response (which should be 'sAN LocAutoStartSavePose 1') and check that the initial pose is still wrong after restart? Thanks!

[koyalbhartia]

Tried the service using Cola-Telegrams and yes the response is as expected as shown in the below screenshot.

And No, the initial pose is still wrong after restart (returns to origin).
I also tried enabling LocAutoStart after restart. (using SickLocSetAutoStartActive service) , yet no luck.

[rostest]

Thanks for your reply! The LocAutoStart itself is still activated after restart? Can you recheck with cola-command

rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocSetAutoStartActive', wait_response_timeout: 1}"

just to make completely sure, that the SIM responds with sAN LocSetAutoStartActive 1?

[koyalbhartia]

I get a different response with the below command as shown in the screenshot:

rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocSetAutoStartActive', wait_response_timeout: 1}"

[rostest]

Sorry, my fault! The correct command is

rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocSetAutoStartActive 1', wait_response_timeout: 1}"

(with 1 to activate and 0 to deactivate). Without the parameter 1, SIM correctly responds error number 8 (which means missing parameter). Can you please try again? Thanks a lot!

[koyalbhartia]

Yes, after restart and running your above command, I get this:

[rostest]

Thanks for checking and confirming!

[rostest]

We will discuss this issue with the internal SICK-Support and will give you a feedback in the next couple of days.

[koyalbhartia]

Cool, thanks. Do let me know if I can provide any any information.

[heinrse]

Hi,

I had a look at the thread and I noticed, that you try to figure out, if the LocAutoStart itself is still activated after restart. This is a very good idea to narrow down the error. Unfortunately you used the wrong command to query the state of the AutoStart function. Instead of query the state you activated the AutoStart function again and the respond is true.
To query if the AutoStart function is still active after restart please use the following command:
"sRN LocAutoStartActive"
Note that this is a variable and to query this variable "sRN" is used instead of "sMN".
In the same way you can also query the interval: Remove "Set" and change "sMN" to "sRN".
"sRN LocAutoStartSavePoseInterval"

In ROS this would be: https://github.com/SICKAG/sick_lidar_localization/blob/master/srv/SickLocAutoStartActiveSrv.srv
If using Cola telegrams directly:
rosservice call SickLocColaTelegram "{cola_ascii_request: 'sRN LocAutoStartActive', wait_response_timeout: 1}"

I guess the answer will be "sRA LocAutoStartActive 0" after restart.
I think maybe you forgot to send "sMN SavePermanent" after "sMN LocSetAutoStartActive 1" and setting the correct interval.
If you do not send SavePermanent befor restart, all settings will be lost. So this is crucial.
You do not have to send SavePermanent after every setting. You only have to send it once after you set everything as you wish.

Please let me know if this solves the issue.
Best regards,
Sebastian

[heinrse]

Here is the corresponding ROS service to save settings permanently:
https://github.com/SICKAG/sick_lidar_localization/blob/master/srv/SickSavePermanentSrv.srv

[koyalbhartia]

Thank you for your reply.

I tried running the following command after doing the initial steps of setting the AutoStart and saving the pose.
rosservice call SickSavePermanent "{}"
however I get an error message: responded with an error: b''.
This tells me that this service expects some setting it doesn't have, or I am not using it the right way.

I think it'll be easier if you could help me with the complete procedure/steps required (full commands so that I don't miss out on any necessary parameter) to achieve the saving and re-initializing at the same pose functionality.

It will also be helpful for anyone else who might face the same issue in future.

Thanks.

[heinrse]

Hey,
Please do the following things:

1. rosservice call SickLocColaTelegram "{cola_ascii_request: 'sEN SCParmChngd 0', wait_response_timeout: 1}"
2. rosservice call SickLocSetAutoStartActive "{active: True}"
3. rosservice call SickLocSetAutoStartSavePoseInterval "{interval: 1}"
4. rosservice call SickSavePermanent "{}"
5. Restart the hardware.
6. Let me know if this solved the issue.

Thanks.

[koyalbhartia]

I tried the above commands and it seems to work great. Thanks for your effort..!!
The robot reinitializes itself to the last known position as you can see in the screenshot however with a little lower quality.

Before restart:

After restart:

I have a few questions and observations.

1. What is SCParmChngd? I did not that find on the github when I was doing my research on achieving the functionality.
2. When I run the first command (SCParmChngd) from your last post, I get the response: cola_ascii_response: "sFA 32", is that the right behavior.
3. SickSavePermanent service call, returns with an error b when "Localizing" is ON, otherwise it returns a success True.

Also, do let me know if you also need me to open this as a ticket on your service portal.

Thanks.

[heinrse]

Hi,

The pose quality after reinitialization is expected to be more or less similar to the pose quality before switching off the SIM. Still it is not the same. The worse the Pose Quality is when switching off the SIM, the less equal it is when reinitilized. Therefore it is recommended to switch off with a good pose quality. Relevant is especially the moderate value of the pose quality. If necessary, please use LocForceUpdate once or twice to improve the pose quality after renitialization.

1 + 2: SCParmChngd: I am still investigating on this. "sFA 32" means unknown command. On your SIM seems to be a special event activated and SCParmChangd can be used to deactivate it. This was not successfull. This event will generate an additional response after you sent SavePermanent which seems to be not caught correctly and therefore causing the error you get when sending SavePermanent.

3. I tried to reproduce this but it was not possible. I could send SavePermanent independant if Localizing is on or not. I think the problem is the special event.

Even if you receive an error, the settings are nevertheless saved permanent. I will come back to you as soon I have a permanent solution.

Please open an additional ticket in our supportportal to discuss the other questions. https://supportportal.sick.com/secure/ticket/add/

Best regards,
Sebastian

[koyalbhartia]

Hi,

Thank you for the response. Yes, LocForceUpdate can be used to improve the quality after reinitialization.

Looks like even though the SCParmChngd was not successful, the robot was able to reinitialize itself as shown in the screenshots of my previous reply. I'll try to get your steps working programatically with and without SCParmChngd, and will open a ticket if I have any further questions.

Meanwhile I tried another approach to achieve the same functionality. I asynchronously save the robot pose received from the result telegram topic in a yaml file (txt file). System restart loads this yaml file into the ROS parameter server. Upon restart, I fetch the pose values from text file using rosparams to reinitialize the robot in the last saved location using the service LocSetPose, and then perform a LocForceUpdate.

However I plan to switch to your permanent solution as I will not have to worry about volume storage of the yaml file during software deployment using images.

Thanks.

[rostest]

Closed due to inactivity.