Define setpoints also as a service

[TSC21]

This offers two possibilities:

1. user control over the vehicle based on the setpoint commands issued
2. test the setpoints part of the firmware before moving to use a planner

I can advance by creating a new script called mavsetp. This one will be divided in 3: local, global and attitude (we still have the possibility of creating a setpoint global plugin). First two offer the services to control pos, vel and accel, while the second one offers control over quat and rpy.

So the command can be issued like:

$ rosrun mavros mavsetp local pos <x> <y> <z>

$ rosrun mavros mavsetp global vel <vx> <vy> <vz>

$ rosrun mavros mavsetp attitude quat <x> <y> <z> <w>

[TSC21]

@vooon how can I add two arguments like local pos or global vel to issue a command, instead of just using one arg as you have on the other srv?

Update: Ok i found out that I can add multiple subargfor that.

[TSC21]

First commit at #127.

[TSC21]

Sent you an e-mail regarding this issue. Can redo it and send a new PR with some inputs from you. Thanks!

[TSC21]

@vooon did you receive my e-mail regarding this issue?

[vooon]

@TSC21 Sorry for long response, I'm kind of busy lately.

Yes, I looked but did not see any difference with PR #127.
And I did not understand what messages you want to send.

[TSC21]

@vooon no problem ;)

Yes there's no difference cause I sent them when I created th PR. What I want to do is send setpoints through the terminal, using cmds similar to this:

$ rosrun mavros mavsetp local pos <x> <y> <z>

to send a local position setpoint;

$ rosrun mavros mavsetp global vel <vx> <vy> <vz>

to send a global velocity setpoint (then I can create the global setpoints plugins);

$ rosrun mavros mavsetp attitude quat <x> <y> <z> <w>

to send a attitude setpoint (with quaternion in this case).

But I don't know how to do this using the argparse of python :S

By the way, I found that with firmware developers that this to work we have to send a Mavlink msg to change the FCU mode to OFFBOARD and for that a valid setpoint must be recieved first in the FCU so that OFFBOARD mode is enabled and the setpoint is received, so this srv needs to:

• First part, send the desired setpoint through the correspondent API (twist for vel, pose for pos, etc)
• Second and last, without delays to the second part, send a cmd to change mode (through the command MAV_CMD_NAV_GUIDED_ENABLE);

As you can see on the code of the setpoints plugins, I added the option to send the setpoints through cmd srv, besides the topic listening they were already doing.

[vooon]

@TSC21 check that example.

[TSC21]

@vooon isn't there missing modifications in setpoint_position.cpp and setpoint_velocity.cpp so they can receive msg from service?

Note: I sent you already the modifications I did on those so it's easier for you to add.

[vooon]

@TSC21 I think topic duplicate not needed. If you mean ROS Service, that you should define it as a service, not topic.

See service advertise: https://github.com/vooon/mavros/blob/master/mavros/src/plugins/command.cpp#L79

But i think it not needed, simple topic publisher + already implemented topics is enough.

[TSC21]

Ok I understand now. I misjudged the safety_area.cpp plugin and I though I had to add a condition so it can also retrieve data from a service call.

[TSC21]

It's turning to be difficult to change to OFFBOARD mode in the FCU. It rejects both MAV_CMD_NAV_GUIDED_ENABLE (which is not implemented yet on offboard2_externalsetpointmessages, the branch where the external setpoints are being handled and forwarded) and setting the mode to OFFBOARD (using or the RC switch or the set command now implemented).

I already left some discussion open in the firmware side so this could be solved.

[TSC21]

Update: the firmware accepts the MAV_CMD_NAV_GUIDED_ENABLE command now (I had loaded the wrong firmware), but returns reject OFFBOARD. Probably we have to check the timing of sending the setpoint and the the mode enable cmd on the service.

[TSC21]

@vooon some corrections have to be made: since the FCU enters in OFFBOARD timeout after 0.5s without receiving any setpoint, enable_offboard should be issued on publish_once, should it should be:

def publish_once(args, pub, msg):
    pub.publish(msg)

    # stick around long enough for others to grab
    timeout_t = rospy.get_time() + _ONCE_DELAY
    while not rospy.is_shutdown() and rospy.get_time() < timeout_t:
        rospy.sleep(0.2)
        enable_offboard(args)
        print_if(pub.get_num_connections() < 1,
             "Mavros not started, nobody subcsribes to", pub.name)

and

def do_local_pos(args):
    pub = rospy.Publisher(args.mavros_ns + "/setpoint/local_position", PoseStamped,
                          queue_size=10, latch=True)
    rospy.init_node("mavsetp", anonymous=True)

    pos = PoseStamped(header=Header(frame_id='mavsetp', stamp=rospy.get_rostime()))
    pos.pose.position = Point(x=args.position[0], y=args.position[1], z=args.position[2])

    publish_once(args, pub, pos)


def do_local_vel(args):
    pub = rospy.Publisher(args.mavros_ns + "/setpoint/cmd_vel", TwistStamped,
                          queue_size=10, latch=True)
    rospy.init_node("mavsetp", anonymous=True)

    vel = TwistStamped(header=Header(frame_id='mavsetp', stamp=rospy.get_rostime()))
    vel.twist.linear = Vector3(x=args.velocity[0], y=args.velocity[1], z=args.velocity[2])

    publish_once(args, pub, vel)

That way it will activate OFFBOARD, issues the setpoint, and goes back to the previous mode (it's entering onto failsafe cause I have not armed the quad and no battery is connected).
I will send you a PR with this correction.

[v-to]

Hi Guys,

I can see it is closed, however I have problems with this issue.
We are working with Rover, where we don't have OFFBOARD.
Why can't we just leave as it was here d73b819
Or maybe we can force enabling GUIDED instead of OFFBOARD in case of Rover?

I can see that in the newest mavros the code is completely different and I am not sure howe are these issues solved there.

What would be your suggestion for us? Keep old version without offboard mode?

Kind regards

Witold