PX4 rotation over z axis low yaw_rate

Question

The goal I want to achieve is to rotate the drone over z axis slow and when it detects an object it will stop rotating. The first node is publishing the string "Searching" and the second node is subscribing to it. So every time it receives "Searching" the drone must rotate.

• Ubuntu 18.04
• ROS melodic
• PX4 firmware
• Python 3.6

I have read part of this code in one paper written in C++ but I am not so good at it, I use python. I would like to ask you guys if you can give me some hints. I want to implement the below code in python.

ros::Duration d(0.5);

geometry_msgs::PoseStamped cmd;
cmd.pose.position.x = 0.0;
cmd.pose.position.y = 0.0;
cmd.pose.position.z = 2.0;
cmd.pose.orientation.x = 0.0;
cmd.pose.orientation.y = 0.0;
cmd.pose.orientation.z = 0.0;
cmd.pose.orientation.w = 1.0;
Eigen::Affine3d t;
ROS_INFO("Searching target...");
while (ros::ok() ) 
{
  if (c == 'q' || rc < 0)
    break;
  tf::poseMsgToEigen (cmd.pose, t);
  t.rotate (Eigen::AngleAxisd (M_PI/10.0, Eigen::Vector3d::UnitZ()));
                // AngleAxisf (angle1   ,        Vector3f::UnitZ())
  tf::poseEigenToMsg(t, cmd.pose);
  nav->SetPoint(cmd);
  ros::spinOnce();
  d.sleep();
}

https://osf.io/jqmk2/

I am not so familiar with C++ but what I understand in the code to rotate the drone, is that, it is taking cmd = PoseStamped as an input and then it is applying rotation to that position (rotation matrix) and the result of this rotation matrix is been publishing as Pose. Is it correct?

I am sending smaller position targets to the pose.orientation.z to go for 0.0 to 2pi with increment of 0.1 to rotate the drone but it is rotating too fast and it's not keeping its x, y position... this is the code:

if data.data == "Searching" and self.yawVal < two_pi:
    rVal, pVal = 0, 0
    pose.position.x = 0
    pose.position.y = 0
    pose.position.z = 1.6
    quat = quaternion_from_euler(rVal, pVal, self.yawVal)
    pose.orientation.x = quat[0]
    pose.orientation.y = quat[1]
    pose.orientation.z = quat[2]
    pose.orientation.w = quat[3]
    pub.publish(pose)
    self.yawVal += 0.1
else:
    self.yawVal = 0

I realize is not a good way because the drone rotate so fast and if I add a sleep the drone cannot recognize an object because it's rotating fast. Is it possible to translate the C++ code to python?