2

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?

Diego
  • 21
  • 2
  • This can of course be converted to python, but it needs to be a more complete example to provide an appropriate response. A rotation is being applied, but nothing is published explicitly in the example. – BTables Sep 24 '21 at 03:28
  • My advice when using ROS for small scripts, when you're not familiar with it, is to use the C++ anyway. ROS topics/messages are strongly typed, at least at run time, but also at compile time if you use C++. Based off of another comment, I think you need to rate limit how often you update your position, based on time, otherwise it'll spin as fast as your computer can process a new position. But I also think based on your comments you're not familiar with how to structure a node in ros, sleeping the main thread for callbacks or [timers](http://wiki.ros.org/roscpp/Overview/Timers) to keep running – JWCS Sep 24 '21 at 16:28
  • Seconding BTables, the link you provided isn't helpful, and you'll need to provide a clearer or smaller question to get a good answer. – JWCS Sep 24 '21 at 16:29
  • Thank you so much guys for your comments. @BTables The full code is availabe but it is too long and it's divide in different sections. – Diego Sep 27 '21 at 10:28
  • 1
    @JWCS Thank you for your comment. I am not familiar with C++ that is why I'm using python. This is small script because I have different nodes for different task, even if the task is so simple. I will take a look how to rate limit. – Diego Sep 27 '21 at 10:31

0 Answers0