Mavros: Joystick teleoperation

I think better not a plugin, but script node that use rc_io plugin.

Will that include setpoint stuff?

I'm not very used to the scripts as you saw, I would really prefer a plugin. But I can send you this as a plugin and then you can convert it to a script if you want.

Just i don't want duplicate code that already exists (and works).

I my mind this node should subscribe to joystick topic and produce rc override messages (or setpoint).
Send me please what data i should send to rc/setpoint and i write script (channel map etc).

Ok I would do that right away

@vooon sent you the e-mail now ;)

Ok, i'll see it tomorrow.

Sure :+1: Thanks!

I could add a similar node that I have for this if it is needed. @TSC21

@tonybaltovski please send it to me. I think it useful example, but perhaps it must be in extras.

@tonybaltovski I already started a plugin, which I sent to @vooon so it can adapt the way he wants. It was almost ready for setpoint control, but RC override part I left it to @vooon.

What's the status on this? I would really like to get an hand on this so to test setpoints also :)

Maybe tomorrow...

@TSC21 It's just proof of concept, but it should work.

I will try it, thanks! :) But my priority is to test the setpoints using the joystick. If you please can add the possibility of position and velocity control I would be grateful.

For velocity, send cmd_vel as TwistStamped to be subscribed by setpoint_velocity;

The position control is simple: instead of cmd_vel it sends position as PoseStamped but this has to be a particular case - it has to do sum the previous position value sent so it can increment or decrement the position (so the values of x,y,z are not limited to -1..1; what's limited is the value that is added to last value; so if the joystick is always sending +1, in each iteration it sums +1 -> the only doubt I have here is if we have to limit the rate in this case, so the iterations don't some up values to fast).
This one is subscribed by setpoint_position.

Update: regarding the /joy topic: I'm not able to retrieve any data from my joystick using the mavteleop script. But using the universal_teleop launch I can. So there must be something missing on the script.

You run the joy_node?

rosrun joy joy_node

2014-08-27 18:17 GMT+04:00 TSC21 [email protected]:

Update: regarding the /joy topic: I'm not able to retrieve any data from
my joystick using the mavteleop script. But using the universal_teleop
launch I can. So there must be something missing on the script.

—
Reply to this email directly or view it on GitHub
https://github.com/vooon/mavros/issues/133#issuecomment-53578566.

No! Yeah it works now. Forgot the joy_node. Can't that be included on the script to be launched?

Ok now regarding the override itself: echoing /mavros/rc/override wasn't getting any data cause you forgot the prefix / here:

override_pub = rospy.Publisher(args.mavros_ns + "/rc/override", OverrideRCIn, queue_size=10)

Need to correct that or I make a PR.
But still there's no RC overriding on the FCU, since the motors don't even change the speed. Will continue to check what's happening.

@vooon RC override it's not working. The script is publishing to /rc/override, which is being subscribed by rc_io.cpp. Echoing /mavros/rc/override gives me channel values changing but nothing happens on the FCU side (no motor spinning changes).

My priority isn't the RC override, even though I would like to set it up. But I can't seem to find what's the problem. In the mean time, can you get the setpoint velocity and position control implemented please? (based on the above comment I did regarding the specs of this kind of control)
Thanks!

I have the same issue with the RC override. The FCU does not use it.

@tonybaltovski can you try to debug the reason? I'm occupied with some other stuff regarding POSCTL.

When i test overrides with APM i found, that it silently drops if override message comes from system_id != SYSID_MYGCS (default: 255).

So i change it to 1 (mavros defaults) and it works. Or we could change mavros system_id.

So what is wrong? system_id or comp_id? What it's needed in case of PX4?

Perhaps PX4 don't handle RC override messages, i can't find handler for MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE message.

Perhaps PX4 don't handle RC override messages, i can't find handler for MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE message.

Yes it seems not. Well too bad, I think we can ask implementation, but I would prefer to get setpoints working. Can you give a hand @vooon please?
Thanks in advance!

@vooon I will give it another try on my APM tomorrow and let you know.

I'll try do it tomorrow.

Thank you very much @vooon!

@TSC21 I almost done, but position control very dirty. Need redo it for constant-time integrating.

Thank you @vooon! :D when you have all done please warn me! I will be glad to test this!

You already can test all modes except -pos (and maybe -vel).

@vooon doing a test now. My axes map is different, that's easy to change, but I see that some channels a reversed. You should add a parameter also to reverse channel, without having to go to the control mode and doins something like z=-yaw.

Note: in my case, my axis map is:

axes_map = {
    'roll': 3,
    'pitch': 2,
    'yaw': 0,
    'throttle': 1
}

and in vel control mode:

twist.twist.linear = Vector3(x=-roll, y=pitch, z=throttle)
twist.twist.angular = Vector3(z=-yaw)

Use ~axes_scale/<name> parameter to reverse (set it to -1.0).

Oh ok, didin't checked that. Thanks!

Can you integrate rosrun joy joy_node on script launch instead of adding it to launch file?

@vooon another thing that needs to be working: the node must be always sending the data, even if it is 0.0, i.e. for example, cmd_vel must always be publishing even if we don't move the swtiches on the joystick, cause OFFBOARDmode has a 0.5s timeout and as to be constantly receiving data to stay in that mode or it reverses to the previous mode!

What I'm seeing is that it when we set a channel to a value, cmd_vel just sends that value once. The PX4 implementation needs to be constantly receiving data, so I think what can be done is cmd_vel sends always the previous channel value, except when it receives the a new value. That new value is stored and it's sent constantly till a new value of the channel is set (using the analog sticks off course).

Add this to your launch file for the joy node <param name="~autorepeat_rate" value="5" />

Thanks @tonybaltovski that worked! But I still can't change the parameters using the launch file. Here's how my launch file looks like:

<launch>
    <!-- vim: ft=roslaunch -->
    <!-- example launch script for PX4 teleop -->

    <arg name="fcu_url" default="serial:///dev/ttyACM0:57600" />
    <arg name="gcs_url" default="" />
    <arg name="tgt_system" default="1" />
    <arg name="tgt_component" default="50" />

    <node pkg="mavros" type="mavros_node" name="mavros" required="true" clear_params="true" output="screen">
        <param name="fcu_url" value="$(arg fcu_url)" />
        <param name="gcs_url" value="$(arg gcs_url)" />
        <param name="target_system_id" value="$(arg tgt_system)" />
        <param name="target_component_id" value="$(arg tgt_component)" />

        <!-- px4 blacklist -->
        <rosparam command="load" file="$(find mavros)/launch/px4_blacklist.yaml" />

        <!-- enable heartbeat send and reduce timeout -->
        <param name="conn_heartbeat" value="5.0" />
        <param name="conn_timeout" value="10.0" />
        <!-- enable mavlink autostart on USB port -->
        <param name="startup_px4_usb_quirk" value="true" />

        <!-- joystick axis parameters -->

        <!-- joystick axis map -->
        <param name="~axes_map/roll" value="3" />
        <param name="~axes_map/pitch" value="2" />
        <param name="~axes_map/yaw" value="0" />
        <param name="~axes_map/throttle" value="1" />

        <!-- joystick axis scale -->
        <param name="~axes_scale/roll" value="-1.0" />
        <param name="~axes_scale/pitch" value="" />
        <param name="~axes_scale/yaw" value="-1.0" />
        <param name="~axes_scale/throttle" value="" />

    </node>

    <node pkg="joy" type="joy_node" name="joy" required="true">
        <param name="~/autorepeat_rate" value="50" />
    </node> 
</launch>

Maybe cause this is a mavros_extras script? But issuing rosparam list / I get:

/joy/autorepeat_rate
/mavros/axes_map/pitch
/mavros/axes_map/roll
/mavros/axes_map/throttle
/mavros/axes_map/yaw
/mavros/axes_scale/pitch
/mavros/axes_scale/roll
/mavros/axes_scale/throttle
/mavros/axes_scale/yaw

So I can't understand why it doesn't change the param.

By the way @vooon, it should be axis and not axes ;) I will do that correction and make a PR if you want.

Update: god, my ignorance. axes is plural of axis (so it's ok as is)

Update: if I do rosparam get /mavros/axes_map/pitch I get 2 as supposed, so I think it may be something on mavteleop.

If I do instead:

load_map(axes_map, '/mavros/axes_map/')
load_map(axes_scale, '/mavros/axes_scale/')
load_map(button_map, '/mavros/button_map/')

on rosrun mavteleop:

[ERROR] [WallTime: 1409327711.720875] bad callback: <function joy_cb at 0xa5ce9cc>
Traceback (most recent call last):
  File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/topics.py", line 682, in _invoke_callback
    cb(msg)
  File "/home/vision/vision_ros_ws/src/mavros/mavros_extras/scripts/mavteleop", line 187, in joy_cb
    pitch = get_axis(joy, 'pitch')
  File "/home/vision/vision_ros_ws/src/mavros/mavros_extras/scripts/mavteleop", line 92, in get_axis
    return j.axes[axes_map[n]] * axes_scale[n]
TypeError: can't multiply sequence by non-int of type 'float'

Ok the problem is solved! Here's what I did:
On the launch file I had:

<!-- joystick axis scale -->
        <param name="~axes_scale/roll" value="-1.0" />
        <param name="~axes_scale/pitch" value="" />
        <param name="~axes_scale/yaw" value="-1.0" />
        <param name="~axes_scale/throttle" value="" />

which was giving the previous error of sequence multiply. changed to:

<!-- joystick axis scale -->
        <param name="~axes_scale/roll" value="-1.0" />
        <param name="~axes_scale/pitch" value="1.0" />
        <param name="~axes_scale/yaw" value="-1.0" />
        <param name="~axes_scale/throttle" value="1.0" />

The subscription was solved with changing mavteleop with this:

load_map(axes_map, '/mavros/axes_map/')
load_map(axes_scale, '/mavros/axes_scale/')
load_map(button_map, '/mavros/button_map/')

Will do a PR soon.

PR done in #143.

I was confused the plural form of the axes.

You apply parameters to wrong node. I plainly did startup script.

https://github.com/vooon/mavros/commit/289fa6c18113f6c0c1e1764cf4e5c37fd5abf9e2

(Started before your PR).

You apply parameters to wrong node. I plainly did startup script.

Yes now I understand! Thanks! Will cancel PR then.

Usage:

1. start FCU connection: roslaunch mavros px4.launch
2. start teleoperation: roslaunch mavros_extras teleop.launch

Or create your own launch with <include file="..."> for both files.

Usage:
start FCU connection: roslaunch mavros px4.launch
start teleoperation: roslaunch mavros_extras teleop.launch

Thanks! Will try then!

Working fine now :) just had to change the yaml and done (check 289fa6c - should be rollbot poll). Now I'll be testing this on GCS HIL to see how it goes. What has to be done on the position part @vooon?

I'm preparing to weekend flights, so it in todo..

I'm preparing to weekend flights, so it in todo..

If you point out myabe I can give a try

I'll fix typo.
Also need add loading RC limits from FCU params.

By the way, just noticed that the PX4Firmware has 500 forks!

By the way, just noticed that the PX4Firmware has 500 forks!

:D Doesn't surprise given the huge community using Pixhawk and PX4 derivative projects.

Just wanted to comment on this. I was about to write something like just as I saw this issue. Its like the community read my mind :D. Tested it, works like a charm.

@pmukherj Can you please outline brief steps on how you got this working on a clean system. I will expand, test and adapt to get this into the wiki.
Thanks.

@mhkabir I think best place in wiki - tutorial pages (currently not exist in mavros space).

@vooon I meant PX4 wiki :) I can add a note to README.

Sure thing! I wanna make sure I didn't take any redundant steps (upgrading the firmware to the dev version may have been not required etc.). I'll write something up today.

@mhkabir I couldn't fly today due to bad weather in Canada, but while bench testing I noticed that when a non-zero r,p,y command is sent (using the joystick) there is no audible change in the motor power. When I control the thrust (through the f710 joystick), I can hear the motors whine up and down. When I swing the body of the quadrotor around, I can hear the motors whine and try and compensate. However, when I give it large a r,p,y command to target through the setpoint/attitude command there is no such compensation heard. Are there any feedback messages implemented as part of mavros, something that acknowledges a command being implemented (maybe the individual motor powers or something?).
I do wanna make sure this is working before I document it, and would rather prove it on a bench then through a crash :P.

@pmukherj Can you please IM me on Gitter or Hangouts ([email protected]). Lets walk through with this. I need more info about your setup.
There is a LOCAL_POSITION_TARGET_NED message and ATTITUDE_TARGET message which aren't in mavros. I'll patch it through as a part of the code refactoring we want to do for the setpoint plugins. Also for these controller target streams, you need to explicitly stream them on PX4 side.
Offboard control is experimental, and this plugin untested, so I am not sure where things go wrong yet. I need more data.

Sorry, Right, I was busy doing some DAQ flights.
So, ATTITUDE_TARGET sounds like exactly what I need to make sure the pixhawk is trying to attain the joystick commands I'm sending it. I have a 3DR, pixhawk with a larger computer mounted onboard (px4 stack works fabulously btw, just missing some flight modes).

I can look into that or do you have a fork with this implemented?

@pmukherj I'm looking into getting attitude and position targets into my visualiser plugin. There are some tf issues, so I'm solving those.
What onboard companion computer are you using?
Regarding the targets, you don't really need to implement anything. Just use the UDP bridge we have on mavros and patch a link through to QGC on your development computer. You can view and plot everything there.

Sorry! Moving offices over new years.
I'm testing with all sorts of computers (odroids, TK1's etc.). I'll take a crack at using a UDP bridge.
I'm guessing I can use any of the telemetry connections into the pixhawk? In addition to the companion computer, I also have a wireless connection into one of the telemetry ports (as is standard with 3DR pixhawk).

Yup. Verified that the setpoints are actually working! The internal setpoints are accepting it, and the controller outputs are also visible. Now for flight tests :). Wish me luck.

@pmukherj You can connect the companion to your TELEM2 port and start it via extras.txt in your microSD card.
Good that the setpoints are working. Best of luck with the flights.
Please check : http://www.pixhawk.org/dev/ros/mavros_offboard
At the bottom there is blank code line, where you should add the mavros command you used to change to offboard mode ( as an option for people if they want to )

Also, can you please add the order for flight testing here : http://www.pixhawk.org/dev/offboard_control/testing

Just put in the points, and I'll clean it up.

@vooon The joystick plugin works fine, but crashes too badly if one of the axes aren't found or out of range, etc.

We can add min/max limitting.

Note that was not a plugin, node.

True ;)
I think I'll do it as a plugin, as the script/python seems rather confusing + unreliable.

I don't want plugin because it's reinventing the wheel. Maximum rewrite from python to C++.
But really not needed when mavteleop runs on desctop and controls OBC's mavros.

@vooon have you thought in a way of solving this?