Joystick teleoperation

[TSC21]

In bound. Will need some help in the RC override part.

[vooon]

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

[TSC21]

Will that include setpoint stuff?

[TSC21]

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.

[vooon]

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).

[TSC21]

Ok I would do that right away

[TSC21]

@vooon sent you the e-mail now ;)

[vooon]

Ok, i'll see it tomorrow.

[TSC21]

Sure :+1: Thanks!

[tonybaltovski]

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

[vooon]

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

[TSC21]

@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.

[TSC21]

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

[vooon]

Maybe tomorrow...

[vooon]

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

[TSC21]

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.

[TSC21]

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.

[vooon]

You run the joy_node?

rosrun joy joy_node

2014-08-27 18:17 GMT+04:00 TSC21 notifications@github.com:

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.

[TSC21]

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

[TSC21]

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.

[TSC21]

@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).

[TSC21]

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!

[tonybaltovski]

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

[TSC21]

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

[vooon]

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.

[TSC21]

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

[vooon]

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

[TSC21]

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!

[tonybaltovski]

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

[vooon]

I'll try do it tomorrow.

[TSC21]

Thank you very much @vooon!

[vooon]

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

[TSC21]

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

[vooon]

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

[TSC21]

@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)

[vooon]

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

[TSC21]

Oh ok, didin't checked that. Thanks!

[TSC21]

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

[TSC21]

@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).

[tonybaltovski]

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

[TSC21]

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.

[TSC21]

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)

[TSC21]

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'

[TSC21]

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.

[TSC21]

PR done in #143.

[vooon]

I was confused the plural form of the axes.

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

[vooon]

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

(Started before your PR).

[TSC21]

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

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

[vooon]

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.

[TSC21]

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

Thanks! Will try then!

[TSC21]

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?