Increase Local Position Publish Rate

Seiwert commented 5 years ago

Feature request

Is your feature request related to a problem? Please describe. The issue I'm encountering is with mapping in GPS denied environments. The TF rate that Mavros/ArduRover produces is too slow for the incoming scan data, especially during rotations.

You can observe pre-rotation and post-rotation generated maps to see how the slow TF rate is affecting the map.

pre-rotation

pre_rotation

post-rotation

post_turn

You can also see my TF tree here:

tf_rate

Describe the solution you'd like Ability to set or increase the local_position publish rate to be higher than 2Hz. In my opinion, the default should be ~10Hz, if that can be supported.

Describe alternatives you've considered I've slowed down the max velocities allowed by the trajectory planner (already pretty slow ~0.3m/s). I've also tried different mapping implementations such as Hector SLAM and GMapping. I'm currently using Octomap. Other implementations had issues with discrepancies in heading information (among other things).

Platform [ ] All [ ] AntennaTracker [ ] Copter [ ] Plane [X] Rover (version 3.4) [ ] Submarine

Additional context For reference, a lot of this work is based off of/similar to this: http://ardupilot.org/dev/docs/ros-slam.html

I'm using laser_scan_matcher for odometry information and pushing that into ArudRover via the vision_pose capability. I'm also using EK3 with IMU and wheel encoders.

I believe this is the code in question: https://github.com/ArduPilot/ardupilot/blob/ee690e7ab92629a84e173a01baf1e7bd439e6ae1/libraries/GCS_MAVLink/GCS_Common.cpp#L2193-L2213 However, I'm unfamiliar with what the process would be to increase this publishing rate. Any assistance in this matter would be greatly appreciated. Thanks!

IamPete1 commented 5 years ago

you should be able to increase the stream rate with the SR1POSITION paramiter (or SR2 or SR3_)

http://ardupilot.org/rover/docs/parameters.html#sr1-position-position-stream-rate-to-ground-station

Seiwert commented 5 years ago

@IamPete1 Thanks for the tip! I believe I need to update the SR2_Position parameter since that is the only one that is configured for 2Hz and reflects what I'm observing (the rest are 10Hz).

However, I'm having issues changing that parameter. Mission Planner is stating that the parameter has been successfully wrote but when I refresh the parameter SR2_Position does not change from 2Hz. I am however able to change SR1_Position parameter. Do you know why this would be?

IamPete1 commented 5 years ago

hum, not sure, have you tried a power cycle? Or it maybe giving you a message that its not happy about something

Seiwert commented 5 years ago

@IamPete1 I've done a power cycle, tried on multiple Pixhawks, tried setting it though UDPCL and direct usb connection, tried setting it while armed and disarmed, and tried setting it to multiple values. It always says that it writes successfully but reverts back to the 2Hz publish rate.

IamPete1 commented 5 years ago

not sure why this would be, not sure what else to suggest, have you tried putting the other SR params back to default and only change the one

Seiwert commented 5 years ago

I think I may have found the issue (although I'm still not quite sure how to fix it).

This looks like the function that sets the stream rates for GCS: https://github.com/ArduPilot/ardupilot/blob/3dc2db8d9a17fa4de92c02cdedf7db119ba86e65/libraries/GCS_MAVLink/GCS_Param.cpp#L123-L202

You can note in that function that there is a set_and_save_ifchanged function and just a set function. I believe that for some reason the set function is only being called on the SR2 stream rate parameters. I've noted that I can set the SR2_Position stream rate to be 10hz, but over time, the rate slowly drops back down to the default of 2hz. You can observe that here:

dropping_rate

I don't know why but after setting the SR2_Position, the position is published at the rate I want. However, the longer it runs the slower it gets. But all I have to do is re-write the stream rate again and it starts publishing again at the faster speed.

I think the root of the problem lies with my lack of understanding with this persist_streamrates function: https://github.com/ArduPilot/ardupilot/blob/master/libraries/GCS_MAVLink/GCS.h#L326

Or I could be completely wrong about all of this. I'm still very new to the raw ArduPilot code base.

It's also worth noting that after I write that SR2_Position parameter, Mission Planner says the parameter was wrote successfully but Mavros is reporting this warning:

[ WARN] [1455212886.812749335]: CMD: Unexpected command 245, result 4

So I guess this may be the not happy message you were expecting, @IamPete1

peterbarker commented 5 years ago

@Seiwert You can increase the rate of a specific mavlink message with SET_MESSAGE_INTERVAL; @rmackay9 has some code in mavros to do just that for IMU data.

I'm also guessing that something is periodically poking the autopilot setting its preferred streamrates. On some vehicles we persist that poking in the streamrate parameters. The 'over time' thing (I hope) may come down to a running average thing in the rate measurements.

Seiwert commented 5 years ago

@peterbarker Thanks for the tip. I found this rosservice that seems to do the trick:

rosservice call /mavros/set_stream_rate "stream_id: 6
message_rate: 10
on_off: true"

Documentation here: http://docs.ros.org/hydro/api/mavros/html/srv/StreamRate.html

However, I've noted that this setting does not persist between runs. Is there no way to permanently set the stream rate?

peterbarker commented 5 years ago

On Thu, 28 Mar 2019, Adam Seiwert wrote:

@peterbarker Thanks for the tip. I found this rosservice that seems to do the trick:

rosservice call /mavros/set_stream_rate "stream_id: 6 message_rate: 10 on_off: true"

Documentation here: http://docs.ros.org/hydro/api/mavros/html/srv/StreamRate.html

However, I've noted that this setting does not persist between runs. Is there no way to permanently set the stream rate?

Anything stored in parameters (SR) directly is persistent - up until the time something changes the set values, which e.g. MAVProxy does all the time* unless you ask it not to.

Some vehicles store streamrate changes persistently in those parameters, some do not.

Note I was talking about SET_MESSAGE_INTERVAL, not streamrates....

Seiwert commented 5 years ago

Hey @peterbarker , sorry about that, I misread your comment and got Stream Rates and Message Intervals mixed up.

A few things:

I was running on an older version of Mavros/Mavlink so I was missing the Mavros _set_messageinterval service. I've fixed that now and can publish to that service.
I attempted to change the message interval of both LOCAL_POSITION Mavlink messages (ID's 32 and 64) but I didn't see a change in the publish rate for local_position in Mavros. I'm not familiar with the difference in stream rate vs. message interval but I believe stream rate is the one I'm looking to change.

Setting the Stream Rate with the above rosservice call has been all that has worked so far. But, I think you called out one of my issues here:

(SR) directly is persistent - up until the time something changes the set values, which e.g. MAVProxy does all the time* unless you ask it not to.

Can you elaborate on the "asking MAVProxy not to" aspect? I've noticed as long as I'm connected the vehicle with Mission Planner, the local_position rate always eventually drops back down to 2Hz. It's not a requirement that I connect with Mission Planner but it would be a nice to have for debugging.

Finally, I think you've called out the root of this entire problem:

Some vehicles store streamrate changes persistently in those parameters, some do not

I believe this vehicle (Aion R1) is one of those vehicles that stores stream rate parameters persistently. I'm totally okay with this, but that circles back around to my previous comment where I mention I'm unable to change this parameter (SR2_POSITION).

So at the end of the day I could write a script that simply makes a call to mavros/set_stream_rate on boot but I'm hopeful that there would be a cleaner solution than that.

peterbarker commented 5 years ago

On Fri, 29 Mar 2019, Adam Seiwert wrote:

I was running on an older version of Mavros/Mavlink so I was missing the Mavros set_message_interval service. I've fixed that now and can publish to that service.

I attempted to change the message interval of both LOCAL_POSITION Mavlink messages (ID's 32 and 64) but I didn't see a change in the publish rate for local_position in Mavros. I'm not familiar with the difference in stream rate vs. message interval but I believe stream rate is the one I'm looking to change.

Which version of ArduPilot are you using? No stable release has support for SET_MESSAGE_INTERVAL in it (the mavros plugin ought to tell you if the command is rejected, but I've no idea if it does!)

message-intervals and streamrates are two sides of the same coin - just different ways of specifying how often you want to get messages. The former is more finely grained, the latter is (sometimes!) persistent.

Setting the Stream Rate with the above rosservice call has been all that has worked so far. But, I think you called out one of my issues here:
  (SR*) directly is persistent - up until the time something changes the set values, which e.g. MAVProxy does all the time unless you ask it not
  to.
Can you elaborate on the "asking MAVProxy not to" aspect. I've noticed as long as I'm connected the vehicle with Mission Planner, the local_position rate always eventually drops back down to 2Hz. It's not a requirement that I connect with Mission Planner but it would be a nice to have for debugging.

Sorry, I'm not familiar with how MissionPlanner does streamrates, but I bet if you look in the telemetry logs you'll find stream rate manipulation from it. Config/Tuning seems to have some options for setting telemetry rates - maybe "-1" tells MissionPlanner not to stuff around with it?

Finally, I think you've called out the root of this entire problem:
  Some vehicles store streamrate changes persistently in those parameters, some do not
I believe this vehicle (Aion R1) is one of those vehicles that stores stream rate parameters persistently. I'm totally okay with this, but that circles back around to my previous comment where I mention I'm unable to change this parameter (SR2_POSITION).

As mentioned - you can set it, but something is handily unsetting it.

Seiwert commented 5 years ago

@peterbarker

Which version of ArduPilot are you using?

FCU: ArduRover V3.4.2 (318a941d) (I haven't upgraded to 3.5 just yet)

(the mavros plugin ought to tell you if the command is rejected, but I've no idea if it does!)
Indeed it does: https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/sys_status.cpp#L1107-L1143 However, it is always returning success at the moment (even with garbage input):
rosservice call /mavros/set_message_interval "message_id: asdfasdfadsf
message_rate: 10.0" 
ERROR: Unable to send request. One of the fields has an incorrect type:
field message_id must be unsigned integer type

srv file: uint32 message_id float32 message_rate

bool success


> As mentioned - you can set it, but something is handily unsetting it.
- Is it possible it may just never be getting saved?
https://github.com/ArduPilot/ardupilot/blob/3dc2db8d9a17fa4de92c02cdedf7db119ba86e65/libraries/GCS_MAVLink/GCS_Param.cpp#L153-L158
- Or if it's getting unset, is there a Stream Rate wizard that could help me figure out why?

peterbarker commented 5 years ago

On Fri, 29 Mar 2019, Adam Seiwert wrote:

FCU: ArduRover V3.4.2 (318a941) (I haven't upgraded to 3.5 just yet)

No support for SET_MESSAGE_INTERVAL on 3.4.2

  (the mavros plugin ought to tell you if the command is rejected, 
but I've no idea if it does!)

Indeed it does: https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/sys_status.cpp#L1107-L1143 However, it is always returning success at the moment (even with garbage input):

rosservice call /mavros/set_message_interval "message_id: asdfasdfadsf message_rate: 10.0" ERROR: Unable to send request. One of the fields has an incorrect type: field message_id must be unsigned integer type

Well, that's an entirely understable error :-)

srv file: uint32 message_id float32 message_rate

bool success
  As mentioned - you can set it, but something is handily unsetting it.
Is it possible it may just never be getting saved? https://github.com/ArduPilot/ardupilot/blob/3dc2db8d9a17fa4de92c02cdedf7db119ba86e65/libraries/GCS_MAVLink/GCS_Param.cpp#L153-L158

Plane and Rover both persist their streamrates, so if it isn't saving that's a bug.

Or if it's getting unset, is there a Stream Rate wizard that could help me figure out why?

Best to look at telemetry logs, as mentioned.

QGC does have a nice telemetry-rate thingy.

Seiwert commented 5 years ago

So @peterbarker you totally called it:

Anything stored in parameters (SR) directly is persistent - up until the time something changes the set values, which e.g. MAVProxy does all the time* unless you ask it not to.

This was exactly my issue. I was looking into telemetry logs like you recommended and I noticed that once I set SR2_POSITION in QGC the value never changed back to 2Hz and stayed at 10Hz. I did some more digging and noticed this inside of Mission Planner:

That Position drop down was repeatedly overriding whatever value I set for the SR2_Position parameter and was essentially re-writing over my write. I changed this to 10Hz and have now been seeing the behavior I expected.

Thanks so much for your help @peterbarker and @IamPete1 !!

piyushy6 commented 4 years ago

I am facing the same issue, my SR2_Position parameter keeps on changing back. I have even tried to change the Telemetry Rates position to -1. But the issue still persists. I am getting GPS data from Pixhawk to Rpi by telemetry 2. How should I then change the sampling rate of the GPS data?

ArduPilot / ardupilot