时间戳

首页 > 解决方案 > 如何将值传递给 ROS 中的发布者？

python - 如何将值传递给 ROS 中的发布者？

问题描述

我目前正在尝试编写一个 Python ROS 程序，该程序可以作为 ROS 节点（使用 rosrun）执行，它实现了在单独的 Python 文件 arm.py 中声明的 defs（可在：https ://github.com/nortega1/dvrk -ros/blob/44c8604b6c120e91f5357e7fd3649a8f7936c504/dvrk_python/src/dvrk/arm.py）。该程序最初检查手臂的当前笛卡尔位置。随后，当提供手臂必须通过的一系列点时，程序计算一个多项式方程，并给定一系列 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)

但是，我不确定如何将 x 和 y 值（我将在以后计算 z 值）传递给我正在创建的 python 程序中的发布者。这是我迄今为止所写的：

#!/usr/bin/env python

import rospy
from tf import transformations
from tf_conversions import posemath
from std_msgs.msg import String, Bool, Float32, Empty, Float64MultiArray
from geometry_msgs.msg import Pose, PoseStamped, Vector3, Quaternion, Wrench, WrenchStamped, TwistStamped

def callback(data):
  rospy.loginfo(data.pose)

def currentPositionListener():
    rospy.init_node('currentPositionListener', anonymous=True)
    rospy.Subscriber('/dvrk/PSM1/position_cartesian_current', PoseStamped, callback)
    rospy.spin()

def lagrange(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 newPositionListener():
    rospy.Subscriber('/dvrk/PSM1/set_position_cartesian', PoseStamped, trajectoryMover)
    rospy.spin()

def trajectoryMover(data):
    points =[(0,0),(45,30),(23,10), (48,0)]
    xlist = [i for i in range(100)]
    ylist = [lagrange(points, xlist[i]) for i in range(100)]
    for x, y in zip(xlist, ylist):
        data.pose.x = x
        data.pose.y = y
        data.pose.z = 0

if __name__ == '__main__':
    currentPositionListener()
    newPositionListener()

任何帮助将不胜感激！

标签： pythonpublish-subscriberos

解决方案

问题出在.trajectoryMover()：

如果你想发布一条Pose消息，你应该创建一条Pose消息并填写它的位置，如下面的代码片段：

from geometry_msgs.msg import Pose


def trajectoryMover(data):
    pose = Pose()
    self.__set_position_cartesian_pub = rospy.Publisher(
        self.__full_ros_namespace + '/set_position_cartesian',
        Pose, latch = True, queue_size = 1
    )

    points =[(0,0),(45,30),(23,10), (48,0)]
    xlist = [i for i in range(100)]
    ylist = [lagrange(points, xlist[i]) for i in range(100)]
    for x, y in zip(xlist, ylist):
        pose.x = x
        pose.y = y
        pose.z = 0
        self.__set_position_cartesian_pub.publish(pose)

[注意]：

  • A.spin()足以满足您的代码。
  • 我无法意识到您需要什么数据。

推荐阅读