I need help with the command CmdVel(roll, pitch, yawrate and thrust).

[Wallat1]

Hi.

Sorry for the inconvenience, but I still have problems with the CmdVel command, so I would like to know if there is no error in the following lines of code

void cmdVelChanged(
const geometry_msgs::Twist::ConstPtr& msg)
{
// if (!m_isEmergency) {
float roll = msg->linear.y;
float pitch = -msg->linear.x;
float yawrate = msg->angular.z;
uint16_t thrust = (uint16_t)msg->linear.z;
m_cf.sendSetpoint(roll, pitch, yawrate, thrust);
// ROS_INFO("cmdVel %f %f %f %d (%f)", roll, pitch, yawrate, thrust, msg->linear.z);
// m_sentSetpoint = true;
// }
}

and in

self.cmdVelPublisher = rospy.Publisher(prefix + "/cmd_vel", geometry_msgs.msg.Twist, queue_size=1)

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)

I would appreciate your help.

Regards.