'/cmd_position' topic

[ntukenmez3]

We'd like to give an initial position to crazyflies via '/cmd_position' (We use cpp backend(update: we use cflib)). Yet, we couldn't see any motion once we've sent the related info for this topic (we haven't seen any error on the terminal). Do we need anything to activate this function? I've added the related part below. Could you help us to fix this one?

publisher definition:

self.cmdPositionPublisher = node.create_publisher(Position, prefix + '/cmd_position', 1)
self.cmdPositionMsg = Position()
self.cmdPositionMsg.header.frame_id = '/world'

the related function:

def cmdPosition(self, pos, yaw=0.):
        
        self.cmdPositionMsg.header.stamp = self.node.get_clock().now().to_msg()
        self.cmdPositionMsg.x = pos[0]
        self.cmdPositionMsg.y = pos[1]
        self.cmdPositionMsg.z = pos[2]
        self.cmdPositionMsg.yaw = yaw
        self.cmdPositionPublisher.publish(self.cmdPositionMsg)

our test command (cmdVel() part works pretty well):

def cf_subscriber_callback(self,msg:LogDataGeneric):
       self.x=...
       self.y=...
       self.z=...
       self.vx=...
       self.vy=...
       self.vz=...
  
       self.get_logger().info("cf3 stateEstimate is processed..."+" x:"+str(self.x)+" y:"+str(self.y)+" z:"+str(self.z)+" vx:"+str(self.vx)+" vy:"+str(self.vy)+" vz:"+str(self.vz))
       self.get_logger().info("initial:"+str(time_1))
       self.time_learning=time.perf_counter()
       if (self.time_learning-time_1)<7.5:
           self.get_logger().info("initial2:"+str(self.time_learning-time_1))
           self.crazyflies[1].cmdPosition(pos=np.array([0.3,0.5,1.0]), yaw=0.0) 
       else:
           self.crazyflies[1].cmdVel(...)

Update:

I've added the following part in crazyflie_server.py to use /cmd_position topic with cflib backend :

self.create_subscription(Position, name + "/cmd_position", partial(self._cmd_position_changed, uri=uri), 10)
.
.
.
def _cmd_position_changed(self, msg, uri=""):
        x = msg.x
        y = msg.y
        z = msg.z
        yaw = msg.yaw
        self.swarm._cfs[uri].cf.commander.send_position_setpoint(x, y,z, yaw)

When I check the topics with rqt, the topics look fair. However, the above cmdPosition() function needs something to activate it since we couldn't see any motion on crazyflie. If you need more info, we can share it here.

[whoenig]

Does it work if you first take-off?

[ntukenmez3]

I actually tried it before. The point is here the high-level commands and low-level commands are not working together, right?
I've made an executable python script to manage all commands together. I've added the related test code in the python script below. All publishers and subscribers are in crazyflie.py script which has been prepared on GitHub. If you invoke the high level commands from
crazyflie.py, they work pretty well. But, the high level commands are not working in the subscriber function (i.e., cf_subscriber_callback(self,msg:LogDataGeneric)). At this point, I guess I need to use both high-level and low level commands. However, I do not know if the transition between high-level and low-level is smooth. Long story short, is it possible to use first high-level then low level-commands?

#!usr/bin/env python3

import rclpy
from rclpy.node import Node
import time
import numpy as np
from crazyflie_py import Crazyswarm
from crazyflie_py.crazyflie import CrazyflieServer, TimeHelper
from crazyflie_py.crazyflie import Crazyflie
from crazyflie_py_1.crazyflie1 import CrazyflieServer1, TimeHelper1
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from std_msgs.msg import Float32
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Point32
from crazyflie_interfaces.msg import LogDataGeneric

import logging

import cflib.crtp  # noqa
from cflib.crazyflie import Crazyflie


TAKEOFF_DURATION = 7.5
LANDING_DURATION = 15.0
HOVER_DURATION = 50.0

class cfLearning():
    def __init__(self):
        rclpy.init()
        self.allcfs = CrazyflieServer()
        self.timeHelper = TimeHelper(self.allcfs)
        cf3 = self.allcfs.crazyflies[1]
        cf3.cmdVel(roll=0.0, pitch=0.0, yawrate=0.0, thrust=0.0)
        self.timeHelper.sleep(TAKEOFF_DURATION + HOVER_DURATION)

def main(args=None):
    swarm=cfLearning()

if __name__ == '__main__':
    main()

[whoenig]

High-level followed by low-level is no problem. The other way around (e.g., landing after using low-level commands), requires a setpoint-notification-stop command in between.

[ntukenmez3]

I just tried cmdPosition() as following under cf_subscriber_callback(self,msg:LogDataGeneric):

 if (self.time_learning-time_1)<15:
             self.crazyflies[1].cmdPosition(pos=np.array([0.0,-0.5,1.5]), yaw=0.0)

The thing is that this command is not working properly. I couldn't catch the reason. However, cmdVel() works pretty well. I thought they eventually use the crazyflie_server.py for self.create_subscription(Twist, name + "/cmd_vel_legacy", partial(self._cmd_vel_legacy_changed, uri=uri), 10) and self.create_subscription(Position, name + "/cmd_position", partial(self._cmd_position_changed, uri=uri), 10) to send crazyflie commands (i.e., self.swarm._cfs[uri].cf.commander.send_setpoint(roll, pitch, yawrate, thrust) and self.swarm._cfs[uri].cf.commander.send_position_setpoint(x, y, z, yaw)).

[ntukenmez3]

I think at this point I should put some comments here. The site https://imrclab.github.io/crazyswarm2/overview.html#support-functionality-with-backends says cflib backend does not support /cmd_position. Even if we add this functionality in crazyflie_server.py, this is not going to send this command signal to crazyflie or not? Because, we have added this part in crazyflie_server.py to use this topic name. The related part can be checked above.