cmd_vel_legacy not working, cflib backend

[GusBon]

Hi,
I am using Crazysim and Crazyswarm2 (with the cflib backend) and I want to use the cmd_vel_legacy topic to fly my drones with roll/pitch/yaw/thrust.

I have written a node in another package to publish on that topic the wanted rpyt according to a wanted position, but when I launch the node (with Crazysim and Crazyswarm2 launched of course), nothing happens.
I have tested Crazysim and Crazyswarm2 with rqt, and I am able to use services and publish on topics. Furthermore I have written another node that command the drone with the cmd_hover topic according to wanted position and everything work perfectly (the drone goes to the wanted position and hover at this position, I got messages on the terminal where I launched Crazyswarm2 saying that the server is receiving messages from the cmd_hover topic).
But when I launched my node that is using the cmd_vel_legacy topic nothing happens on the simulator (rotors are not turning etc...), and there is nothing on the terminal window where I launched Crazyswarm2. What is really weird is that when I echo the cmd_vel_legacy topic on another terminal, I saw that messages are published on the topic.

Moreover I think that I haven't understand links between different packages of the project :
A callback of the cmd_vel_legacy is created here in the crazyflie server (cflib version):

crazyswarm2/crazyflie/scripts/crazyflie_server.py

Lines 971 to 981 in 6a3bb4b

	def _cmd_vel_legacy_changed(self, msg, uri=""):
	"""
	Topic update callback to control the attitude and thrust
	of the crazyflie with teleop
	"""
	roll = msg.linear.y
	pitch = -msg.linear.x
	yawrate = msg.angular.z
	thrust = int(min(max(msg.linear.z, 0, 0), 60000))
	self.swarm._cfs[uri].cf.commander.send_setpoint(
	roll, pitch, yawrate, thrust)

The subscription is created in the same code:

crazyswarm2/crazyflie/scripts/crazyflie_server.py

Lines 265 to 269 in 6a3bb4b

	self.create_subscription(
	Twist, name +
	"/cmd_vel_legacy", partial(self._cmd_vel_legacy_changed,
	uri=uri), 10
	)

But I didn't find anything in the crazyflie.py code of the crazyflie_py package that is related with this topic (appart for this commented part):

crazyswarm2/crazyflie_py/crazyflie_py/crazyflie.py

Lines 624 to 657 in 6a3bb4b

	# def cmdVel(self, roll, pitch, yawrate, thrust):
	# """Sends a streaming command of the 'easy mode' manual control inputs.
	# The (absolute roll & pitch, yaw rate, thrust) inputs are typically
	# used for manual flying by beginners or causal pilots, as opposed to the
	# 'acrobatic mode' inputs where roll and pitch rates are controlled
	# instead of absolute angles. This mode limits the possible maneuvers,
	# e.g. it is not possible to do a flip because the controller joystick
	# would need to rotate 360 degrees.
	# For more information on streaming setpoint commands, see the
	# :meth:`cmdFullState()` documentation.
	# !NOTE!: The angles and angular velocities in this command are in
	# degrees, whereas they are in radians for cmdFullState.
	# TODO: should we change the name from cmdVel to something else?
	# IMO (japreiss), cmdVel implies controlling linear velocity.
	# Args:
	# roll (float): Roll angle. Degrees. Positive values == roll right.
	# pitch (float): Pitch angle. Degrees. Positive values == pitch
	# forward/down.
	# yawrate (float): Yaw angular velocity. Degrees / second. Positive
	# values == turn counterclockwise.
	# thrust (float): Thrust magnitude. Non-meaningful units in [0, 2^16),
	# where the maximum value corresponds to maximum thrust.
	# """
	# msg = geometry_msgs.msg.Twist()
	# msg.linear.x = pitch
	# msg.linear.y = roll
	# msg.angular.z = yawrate
	# msg.linear.z = thrust
	# self.cmdVelPublisher.publish(msg)

So I don't really understand the goal of the crazyflie package and crazyflie_py package. I don't rally understand the links between those two packages. And I think this is the source of my errors that lead to my malfunctioning node.

Sorry for the long question, but I am a bit lost x)

[knmcguire]

About the goal of the crazyflie and crazyflie_py package, that is more a topic for @whoenig.

About the rpyt not working, if you put a print in the cmd_vel_legacy_changed, do you see anything happening?

[GusBon]

So I have added logging info:

def _cmd_vel_legacy_changed(self, msg, uri=""):
        """def _cmd_vel_legacy_changed(self, msg, uri=""):
        """
        Topic update callback to control the attitude and thrust
            of the crazyflie with teleop
        """
        roll = msg.linear.y
        pitch = -msg.linear.x
        yawrate = msg.angular.z
        thrust = int(min(max(msg.linear.z, 0, 0), 60000))
        self.swarm._cfs[uri].cf.commander.send_setpoint(
            roll, pitch, yawrate, thrust)
        self.get_logger().info(f"{uri}: Received cmd_vel_legacy topic {roll} {pitch} {yawrate} {thrust}")

        Topic update callback to control the attitude and thrust
            of the crazyflie with teleop
        """
        roll = msg.linear.y
        pitch = -msg.linear.x
        yawrate = msg.angular.z
        thrust = int(min(max(msg.linear.z, 0, 0), 60000))
        self.swarm._cfs[uri].cf.commander.send_setpoint(
            roll, pitch, yawrate, thrust)
        self.get_logger().info(f"{uri}: Received cmd_vel_legacy topic {roll} {pitch} {yawrate} {thrust}")

And as expected I received the correct values on the terminal where i launched Crazyswarm2:

Compared to another terminal where I echo my topic this seems consistent:

(I have pushed values of my static gains of my controller to see if something happens, but propellers are not turning on Crazysim)

[knmcguire]

Seems like the cflib backend is working but the connection with crazysim (which we don't maintain ourselves) is not working. If the props weren't running on a real Crazyflie, that would have been a different story.

I believe that Crazysim are maintaining their own fork of the crazyflie python library, so you'll need to pip install that from source. Moreover, I don't know if they actually have support for rpyt control in crazysim.... You can follow their instructions here and open up an issue on their repo if it is not working. https://github.com/gtfactslab/CrazySim

[GusBon]

Ok,
I installed the forked version of the CFlib by following their instructions, so I will open an issue and try to get this functioning.
Thanks a lot for your answers !

[GusBon]

So I opened an issue on the Crazysim github (gtfactslab/CrazySim#9)
And the problem was that i didn't know that it is mandatory to send zeros to switch on low level commander.
It is now working by doing so.
Thanks a lot

[knmcguire]

ah yes good that you got your answer there, and thanks for sharing!