How to use velocity command in Crazyswarm2

[gaurav713914]

We want to control crazyflies using velocity commands in crzyswarm2. I tried uncommenting the cmdVelocityWorld in the crazyflie_py/crazyflie_py/crazyflie.py line 557 to 554 but I'm getting several errors when I am running the script that uses crazyswarm.

Can you please suggest a way to use velocity commands in crazyswarm?

[whoenig]

We have not implemented that directly, yet. It wouldn't be hard, but requires changes in the backend, not only the Python layer. Depending on your use-case cmd_full_state might be sufficient, i.e., you model a single-integrator model in the host side and send position and velocities to the UAV (and set accelerations to zero).

[gaurav713914]

Thank you for the information. I'm closing the issue now. Thank you very much.

[knmcguire]

Take a look at the ROS2 tutorials which. The teleoperation keyboard example shows velocity control with the crazyflie with an additional node and the cflib backend

[gaurav713914]

We want to actually implement unicycle and single integrator kind of robot models on crazyflie through script, is there any example which directly implements unicycle or single integrator robot in crazyflie? There was motion_commander in cflib when we used flowdeck for unicycle model but now we have Qualisys motion capture system and we want to implement unicycle model. I do not know much of ROS hence we are facing problems changing backend things in crazyflie server and msgs.

Please help if you could provide some direct function for implementing a unicycle model, it would be a great help.

[knmcguire]

So the velmux example eventually talks to the cmd_hover topic, which uses a function that the motion commander class of the cflib also uses:

https://github.com/IMRCLab/crazyswarm2/blob/948302919af99daf8b434207a32d88b9b1544d20/crazyflie/scripts/crazyflie_server.py#L846

About the unicycle model, that is a different question that you should ask in the discussions of crazyswarm2: https://github.com/IMRCLab/crazyswarm2/discussions, so that perhaps the community can participate. That is also the main place for support.

[gaurav713914]

This is the thing we actually want, we can use this to operate the crazyflies in the unicycle mode. Am I right? Because unicycle model needs only vx and yaw_rate which this cmd_hover topic is able to produce.

[knmcguire]

Yes exactly, then that is what you can use. Take a look at the code, and let us know if it works out. This piece of code hasn't been tested for a while so I hope it still works!

[gaurav713914]

Ok, I will let you know soon maybe in a few hours. I want to ask the thing we need to do is create a publisher of cmd_hover and publish the values, right?

I have a few doubts, how do we simply change these lines in vel_mux to just create a publisher and publish the velocities?

class VelMux(Node):
    def __init__(self):
        super().__init__('vel_mux')
        self.declare_parameter('hover_height', 0.5)
        self.declare_parameter('robot_prefix', '/cf1')
        self.declare_parameter('incoming_twist_topic', '/cmd_vel')

        self.hover_height  = self.get_parameter('hover_height').value
        robot_prefix  = self.get_parameter('robot_prefix').value
        incoming_twist_topic  = self.get_parameter('incoming_twist_topic').value

        self.subscription = self.create_subscription(
            Twist,
            incoming_twist_topic,
            self.cmd_vel_callback,
            10)
        self.msg_cmd_vel = Twist()
        self.received_first_cmd_vel = False
        timer_period = 0.1
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.takeoff_client = self.create_client(Takeoff, robot_prefix + '/takeoff')
        self.publisher_hover = self.create_publisher(Hover, robot_prefix + '/cmd_hover', 10)
        self.land_client = self.create_client(Land, robot_prefix + '/land')
        self.cf_has_taken_off = False

        self.takeoff_client.wait_for_service()
        self.land_client.wait_for_service()

        self.get_logger().info(f"Velocity Multiplexer set for {robot_prefix}"+
                               f" with height {self.hover_height} m using the {incoming_twist_topic} topic")

    def cmd_vel_callback(self, msg):
        self.msg_cmd_vel = msg
        # This is to handle the zero twist messages from teleop twist keyboard closing
        # or else the crazyflie would constantly take off again.
        msg_is_zero = msg.linear.x == 0.0 and msg.linear.y == 0.0 and msg.angular.z == 0.0 and msg.linear.z == 0.0
        if  msg_is_zero is False and self.received_first_cmd_vel is False and msg.linear.z >= 0.0:
            self.received_first_cmd_vel = True

    def timer_callback(self):
        if self.received_first_cmd_vel and self.cf_has_taken_off is False:
            req = Takeoff.Request()
            req.height = self.hover_height
            req.duration = rclpy.duration.Duration(seconds=2.0).to_msg()
            self.takeoff_client.call_async(req)
            self.cf_has_taken_off = True
            time.sleep(2.0)        
        if self.received_first_cmd_vel and self.cf_has_taken_off:
            if self.msg_cmd_vel.linear.z >= 0:
                msg = Hover()
                msg.vx = self.msg_cmd_vel.linear.x
                msg.vy = self.msg_cmd_vel.linear.y
                msg.yaw_rate = self.msg_cmd_vel.angular.z
                msg.z_distance = self.hover_height
                self.publisher_hover.publish(msg)
            else:
                req = Land.Request()
                req.height = 0.1
                req.duration = rclpy.duration.Duration(seconds=2.0).to_msg()
                self.land_client.call_async(req)
                time.sleep(2.0)        
                self.cf_has_taken_off = False
                self.received_first_cmd_vel = False

It seems to me that we do not require most of these lines because we already takeoff before sending the velocity commands, the lines looking of use to me are:

                self.publisher_hover = self.create_publisher(Hover, robot_prefix + '/cmd_hover', 10)
                msg = Hover()
                msg.vx = self.msg_cmd_vel.linear.x
                msg.vy = self.msg_cmd_vel.linear.y
                msg.yaw_rate = self.msg_cmd_vel.angular.z
                msg.z_distance = self.hover_height
                self.publisher_hover.publish(msg)

I am sorry if I am asking too many basic questions because actually I am new to ROS and do not know much of it.

[knmcguire]

No worries! Just as a tip, it might be good to do the basic tutorials of ROS2 if you haven't already: https://docs.ros.org/en/humble/Tutorials.html. I think that would be a good start anyhow if you have the time)

So what vel_mux.py does, is to listen to cmd_vel (a common topic given out by staple ROS packages), handle a take off by using the service /takeoff when a up velocity is received, then transfers the velocity commands from cmd_vel to cmd_hover, and lands if the cmd_vel message has a down velocity. This is just to make sure not to give a forward velocity command if the crazyflie is not yet in the air.

This means, if you are already handling the take off and landing from your side, you don't need vel_mux. You can just publish the type Hover message to the cmd_hover topic.

If you'd like to use twist instead, no problem in that too. Then just remove the full state machine in def timer_callback() in vel_muc.py and replace with only:

    def timer_callback(self):
                msg = Hover()
                msg.vx = self.msg_cmd_vel.linear.x
                msg.vy = self.msg_cmd_vel.linear.y
                msg.yaw_rate = self.msg_cmd_vel.angular.z
                msg.z_distance = self.hover_height
                self.publisher_hover.publish(msg)

Then you have a simple node that changes the published twist message to a Crazyswarm2 specific Hover message :)

This is indeed the right part of the code that you already indicated.

[gaurav713914]

Thank you for your help regarding the same, I will just edit my main code and test if this hover model is working or not. We are actually targeting an experimental publication that is why we want these models to be implemented on crazyflie as soon as possible. There are a lot of problems being faced by us but we are still trying to learn and overcome those. I am a masters student in Cyber Physical Systems. I will get back to you soon with result or maybe more questions :)

[knmcguire]

Sounds good! I think we should probably close this comment for now, and if you have a follow up question let's handle that on discussions if that is okay? Usually issues are used for adding new features, but since the feature you asked for already existed we can consider this done.

Good luck!