為什么ROS Publisher不發布值？

Question

我目前正在嘗試編寫一個可以作為ROS節點（使用rosrun）執行的Python ROS程序，該程序實現在單獨的Python文件arm.py中聲明的def（可從以下網址獲取： https : //github.com/nortega1/dvrk -ros / .. ）。 該程序最初檢查手臂的當前笛卡爾位置。 隨后，當提供手臂必須經過的一系列點時，程序將計算一個多項式方程，並在給定范圍x值的情況下，程序會對該方程進行評估以找到相應的y值。

在arm.py文件中，有一個發布程序set_position_cartesian_pub ，用於設置手臂的笛卡爾位置，如下所示：

self.__set_position_cartesian_pub = rospy.Publisher(self.__full_ros_namespace + '/set_position_cartesian', Pose, latch = True, queue_size = 1)

問題是發布者set_position_cartesian沒有將newPose的值發布給機器人-任何人都可以找出問題所在嗎？ 我可以確認def lagrange正確計算了x和y坐標的值，這些值通過命令rospy.loginfo（newPose）打印到終端。 由於我過去2天一直在努力解決此問題，因此我們將不勝感激！

#! /usr/bin/python
import rospy
import sys
from std_msgs.msg import String, Bool, Float32
from geometry_msgs.msg import Pose
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Vector3
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Wrench

class example_application:

def callback(self, data):
  self.position_cartesian_current = data.pose
  rospy.loginfo(data.pose)

def configure(self,robot_name):
    self._robot_name = 'PSM1'
    ros_namespace = '/dvrk/PSM1'
    rospy.Subscriber('/dvrk/PSM1/position_cartesian_current', PoseStamped, self.callback)
    self.set_position_cartesian = rospy.Publisher('/dvrk/PSM1/set_position_cartesian', Pose, latch=True, queue_size = 10)
    rospy.sleep(3)
    rospy.init_node('listener', anonymous=True)
    rospy.spin()

def lagrange(self, f, x):
 total = 0
 n = len(f)
 for i in range(n):
  xi, yi = f[i]
  def g(i, n):
   g_tot = 1
   for j in range(n):
    if i == j:
     continue
    xj, yj = f[j]
    g_tot *= (x - xj) / float(xi - xj)

   return g_tot

  total += yi * g(i, n)
 return total

def trajectoryMover(self):
    newPose = Pose()
    points =[(0.0156561,0.123151),(0.00715134,0.0035123151),(0.001515177,0.002123151),(0.0071239751,0.09123150)]
    xlist = [i*0.001 for i in range(10)]
    ylist = [self.lagrange(points, xlist[i])*0.001 for i in range(10)]
    for x, y in zip(xlist, ylist):
        newPose.position.x = x
        newPose.position.y = y
        newPose.position.z = 0.001
        newPose.orientation.x = 0.001
        newPose.orientation.y = 0.001
        newPose.orientation.z = 0.005
        newPose.orientation.w = 0.002
        rospy.sleep(1)
        self.set_position_cartesian.publish(newPose)
        rospy.loginfo(newPose)
        rospy.spin()

def run(self):
    # self.home()
    self.trajectoryMover()

if __name__ == '__main__':
    try:
        if (len(sys.argv) != 2):
            print(sys.argv[0] + ' requires one argument, i.e. name of dVRK arm')
    else:
        application = example_application()
        application.configure(sys.argv[1])
        application.run()

except rospy.ROSInterruptException:
    pass

Answer 1

您未發布，因為調用application.configure()時代碼在rospy.spin()處停止。 根據我對您要執行的操作的了解，該代碼將向一個主題發布10個姿勢，那么您就不再需要它。

我已經移動了rospy.spin()的位置，但是代碼需要的修訂更多。

#! /usr/bin/python
import rospy
import sys
from std_msgs.msg import String, Bool, Float32
from geometry_msgs.msg import Pose
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Vector3
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Wrench

class example_application(object):
    def callback(self, data):
        self.position_cartesian_current = data.pose
        rospy.loginfo(data.pose)

    def configure(self,robot_name):
        self._robot_name = 'PSM1'
        ros_namespace = '/dvrk/PSM1'
        rospy.Subscriber('/dvrk/PSM1/position_cartesian_current', PoseStamped, self.callback)
        self.set_position_cartesian = rospy.Publisher('/dvrk/PSM1/set_position_cartesian', Pose, latch=True, queue_size = 10)

    def lagrange(self, f, x):
        total = 0
        n = len(f)
        for i in range(n):
            xi, yi = f[i]
            def g(i, n):
                g_tot = 1
                for j in range(n):
                    if i == j:
                        continue
                    xj, yj = f[j]
                    g_tot *= (x - xj) / float(xi - xj)

                return g_tot

            total += yi * g(i, n)
        return total

    def trajectoryMover(self):
        newPose = Pose()
        points =[(0.0156561,0.123151),(0.00715134,0.0035123151),(0.001515177,0.002123151),(0.0071239751,0.09123150)]
        xlist = [i*0.001 for i in range(10)]
        ylist = [self.lagrange(points, xlist[i])*0.001 for i in range(10)]
        for x, y in zip(xlist, ylist):
            newPose.position.x = x
            newPose.position.y = y
            newPose.position.z = 0.001
            newPose.orientation.x = 0.001
            newPose.orientation.y = 0.001
            newPose.orientation.z = 0.005
            newPose.orientation.w = 0.002
            self.set_position_cartesian.publish(newPose)
            rospy.loginfo(newPose)

    def run(self):
        # self.home()
        self.trajectoryMover()

if __name__ == '__main__':
    if (len(sys.argv) != 2):
        print(sys.argv[0] + ' requires one argument, i.e. name of dVRK arm')
    else:
        application = example_application()
        application.configure(sys.argv[1])
        application.run()

    try:
        rospy.spin()
    except KeyboardInterrupt:
        rospy.loginfo("Keyboard Interrupt")

考慮到：

• 將腳本參數作為節點的參數。
• 將configure方法移動到__init__方法。
• 將g()函數置於lagrange()之外。

最好使用相對主題名稱，而不要使用絕對主題名稱（絕對主題名稱以/開頭，例如： '/dvrk/PSM1' ）。