cf.cmdVel(self, roll, pitch, yawrate, thrust) in sımulation (SIL) #832

...
 self.cmdVelPublisher = node.create_publisher(Twist, prefix + '/cmd_vel_legacy', 1)
...
def cmdVel(self, roll, pitch, yawrate, thrust):        
        cmdVelMsg= Twist()
        cmdVelMsg.linear.x = float(pitch)
        cmdVelMsg.linear.y = float(roll)
        cmdVelMsg.linear.z = float(thrust)
        cmdVelMsg.angular.z = float(yawrate)
        self.cmdVelPublisher.publish(cmdVelMsg)
        

def _cmd_vel_legacy_changed(self, msg, name=''):
        """
        Translates Twist messages into Crazyflie setpoints.
        msg.linear.x  -> Pitch
        msg.linear.y  -> Roll
        msg.angular.z -> Yaw Rate
        msg.linear.z  -> Thrust
        """
        # 1. Extract values from the Twist message
        roll  = msg.linear.y
        pitch = msg.linear.x
        yaw_rate = msg.angular.z
        thrust = int(max(0, min(65535, msg.linear.z))) # Usually needs to be a 16-bit integer

        # 2. Send the setpoint via the Crazyflie Link
        # Note: The specific API call depends on which library version you are using
        #self.get_logger().info(f'Available methods: {dir(self.cfs[name])}')
        try:
            self.cfs[name].cmdVel(roll, pitch, yaw_rate, thrust)
        except Exception as e:
            self.get_logger().error(f'Failed to send setpoint: {e}')

def cmdVel(self, roll, pitch, yaw_rate, thrust):
        """
        Manually added to bridge the gap between SIL and physical API.
        Maps legacy attitude commands to the simulation setpoint.
        """
        # In SIL, 'self.setpoint' is likely the object used by the internal loop
        firm.plan_stop(self.planner)
        self.mode = CrazyflieSIL.MODE_LOW_FULLSTATE
        self.setpoint.attitude.roll = float(roll)
        self.setpoint.attitude.pitch = float(pitch)
        self.setpoint.attitudeRate.yaw = float(yaw_rate)
        self.setpoint.thrust = float(thrust)
        
        self.cmdHl_yaw = np.radians(self.state.attitude.yaw)
        self.cmdHl_pos = firm.mkvec(self.state.position.x, 
                                    self.state.position.y, 
                                    self.state.position.z)
        # 2. Tell the firmware HOW to interpret these values
        # modeAbs (2) means "Angle", modeRate (1) means "Angular Velocity"
        self.setpoint.mode.roll = firm.modeAbs
        self.setpoint.mode.pitch = firm.modeAbs
        self.setpoint.mode.yaw = firm.modeVelocity
        
        # 3. Disable coordinate-based positioning for manual flight
        self.setpoint.mode.x = firm.modeDisable
        self.setpoint.mode.y = firm.modeDisable
        self.setpoint.mode.z = firm.modeDisable
    
        # Crucial: Set the mode so the simulator knows to use these values
        # Based on your dir() list, 'MODE_LOW_FULLSTATE' or similar might be needed
        # But often, simply updating the setpoint object is enough for the next simulation tick.

import rclpy
from crazyflie_py import Crazyswarm

def main():
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper
    cf = swarm.allcfs.crazyflies[0]
    #cf.cmdVel(roll=0.0, pitch=0.0, yawrate=0.0, thrust=0)
    #cf.cmdVel(roll=0.0, pitch=0.0, yawrate=0.0, thrust=0)

    print("Taking off...")
    cf.takeoff(targetHeight=1.0, duration=2.0)
    timeHelper.sleep(4.0)

    print("Sending thrust...")
    # Roll=20.0 degrees, Pitch=0, Yawrate=0, Thrust=42000
    for _ in range(250): # 5 seconds at 50Hz
        cf.cmdVel(roll=0.0, pitch=0.0, yawrate=0.0, thrust=48000)
        timeHelper.sleepForRate(50)

    print("Landing...")
    cf.land(targetHeight=0.0, duration=2.0)
    timeHelper.sleep(2.0)

if __name__ == "__main__":
    main()