2

我想要实现的目标是在 z 轴上缓慢旋转无人机，当它检测到物体时它将停止旋转。第一个节点发布字符串“Searching”，第二个节点订阅它。因此，每次收到“搜索”时，无人机都必须旋转。

  • Ubuntu 18.04
  • ROS旋律
  • PX4 固件
  • 蟒蛇 3.6

我在一篇用 C++ 编写的论文中阅读了部分代码，但我不太擅长，我使用 python。想请教各位大神，能不能给点提示。我想在 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/

我对 C++ 不太熟悉，但我在旋转无人机的代码中理解的是，它cmd = PoseStamped作为输入，然后将旋转应用于该位置（旋转矩阵），并且该旋转矩阵的结果是发布为姿势。这是对的吗？

我正在向pose.orientation.z发送较小的位置目标以0.0到2pi，增量为0.1来旋转无人机，但它旋转得太快并且没有保持其x，y位置......这是代码：

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

我意识到这不是一个好方法，因为无人机旋转得如此之快，如果我添加睡眠，无人机将无法识别物体，因为它旋转得很快。是否可以将 C++ 代码转换为 python？

4

0 回答 0