0

我有这个从节点（订阅）接收数据的代码，然后在这个节点内我做了一些数学运算，然后我想将数据发布到某个主题。

#!/usr/bin/env python

import rospy
from std_msgs.msg import String, Float32
from math import cos,sin

import sys
import os
import logging
from types import *
import time

class exact_pose():

    def callback_car_yaw(self, data):
        self.car_yaw = data

    def callback_line_pose(self,data):
        self.line_pose = data

        
    def init(self):
      
 
        self.car_yaw = None
        self.line_pose = None
      

    def subscriber(self):
        
 
            
        self.car_yaw_sub = rospy.Subscriber('~car/yaw',Float32, self.callback_car_yaw)
            
        self.line_pose_sub = rospy.Subscriber('~line_pose',Float32, self.callback_line_detect_xL15)

    def publisher(self):

        self.exact_pose_pub = rospy.Publisher('exact_pose',Float32, queue_size=10)
        

        rospy.Rate(1)
        rospy.spin()
      
      
        #sloving for exact pose 

    def calculation(self,):
        while not rospy.is_shutdown():

            self.exact_pose = self.line_pose_sub * cos(self.car_yaw_sub) + self.line_pose_sub * sin(self.car_yaw_sub)
            info1 = Float32()
            info1.data = self.exact_pose
            self.exact_pose_pub.publish(info1)


            rospy.Rate.sleep()
            rospy.spin()


if __name__ == '__main__':
    rospy.init_node('exact_pose', anonymous=True)
    try:
      ne = exact_pose()

    except rospy.ROSInterruptException: pass

所以代码做了什么，从前一个节点订阅一个主题，然后对主题内的这些数据进行一些数学运算，然后在单独的节点上发布最后一个结果

4

1 回答 1

1

所以实际上你应该编写这样一个代码的方式是你在初始化例程中注册订阅者和发布者。订阅者继续在单独的线程中工作：每次向他们发送新数据时，他们将启动回调函数（在这种情况下，它将消息数据保存到类内的本地副本中）。这是独立于主线程完成的。主线程进入一个无限循环（只要 ROS 正在运行），其中它获取数据（从其他订阅者线程写入）定期对其执行计算并以预定义的速率发布它。

但不能保证相同的数据不会被处理两次。如果这不是您需要的，那么请查看其他通信方法，例如 ROS 服务和 actionlib。ROS 主题可能会被删除等。其他通信方法虽然要复杂得多。

您的代码有几个错误。你很幸运，Python 被翻译而不是编译，它从未进入相关的代码部分。

  • self.car_yaw并且self.car_yaw实际上应该用浮点数而不是用None.
  • 和之类subscriber的函数永远不会在您的调用中被调用。publishercalculation
  • ros.Rate.sleep()并且在例程rospy.spin()中无事可做。publisher只要 ROS 正在运行，它们就用于将线程置于无限循环中。这仅在程序内部发生一次，并且在主循环内部发生！
  • exact_pose实际上必须使用类中的本地值，self.line_pos而不是像订阅者这样self.line_pose_sub的订阅者，只有在接收到新数据时才会执行。您不能强迫订阅者以您尝试的方式检查新数据！
  • while并且ros.spin()不能组合使用。在 C++ 中有ros::spinOnce()但在 Python 中没有等价物。因此，您需要使用whileandrospy.Rate.sleep(d)来代替！
  • rospy.Rate.sleep需要一个持续时间作为第一个输入参数，并且不能为空。
  • 你包含了很多你的代码实际上不需要的库。

我尝试对其进行重组，并且以下代码有效：

#!/usr/bin/env python

import rospy
from std_msgs.msg import Float32
from math import cos,sin

class exact_pose():
  def __init__(self):
    # Set default values
    self.car_yaw = 0.0
    self.line_pose = 0.0

    # Initialise node
    rospy.init_node("emergency_procedure", anonymous=True)
    r = rospy.Rate(1)

    # Register topics
    self.car_yaw_sub = rospy.Subscriber('car/yaw',Float32, self.callback_car_yaw)
    self.line_pose_sub = rospy.Subscriber('line_pose',Float32, self.callback_line_pose)
    self.exact_pose_pub = rospy.Publisher('exact_pose',Float32, queue_size=10)

    # Repeat the following code in a loop
    while not rospy.is_shutdown():
      # Perform some calculation and publish to topic
      self.exact_pose = self.line_pose * cos(self.car_yaw) + self.line_pose * sin(self.car_yaw)
      info1 = Float32()
      info1.data = self.exact_pose
      self.exact_pose_pub.publish(info1)
      # Sleep for some time
      rospy.Rate.sleep(r)
    return;

  def callback_car_yaw(self, data):
    # Copy data to class
    self.car_yaw = data
    return;

  def callback_line_pose(self,data):
    # Copy data to class
    self.line_pose = data
    return;

if __name__ == '__main__':
  try:
    # Create instance of class when node is called
    e = exact_pose()
  except rospy.ROSInterruptException:
    pass

将此代码放入您的代码中的.py-file 中，打开一个新控制台并使用 获取工作区source devel/setup.bash，然后启动roscore，打开另一个控制台，再次获取工作区并启动您的 Python 节点rosrun <package> <node>.py。最后启动另一个控制台，再次获取工作区并输出 Python 节点发布的主题rostopic echo /exact_pose。如果没有其他节点正在运行，这应该0.0每秒输出一次。

于 2021-05-27T20:17:43.937 回答