python - 为什么 ROS Publisher 不发布值?
问题描述
我目前正在尝试编写一个 Python ROS 程序,该程序可以作为 ROS 节点(使用 rosrun)执行,该程序实现在单独的 Python 文件 arm.py 中声明的 defs(可在: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
解决方案
您没有发布,因为代码在rospy.spin()
您调用时停止application.configure()
。据我了解您正在尝试做什么,该代码将向一个主题发布 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'
。
推荐阅读
- android - mainUIThread 和数据库访问问题
- python - 如何在运行时初始化 peewee SQLite 数据库?
- regex - 文件字符匹配和添加字符shell脚本
- javascript - Tab-navigation v5 标题中特定选项卡屏幕的渲染按钮
- javascript - CSV 文件阅读器模块
- oracle - SQLPLUS 没有从 ewallet.p12 文件中获取凭据
- php - 在 PHP 中连接 PayPal 以进行用户身份验证
- laravel - 有没有办法获取我在 s3 存储桶上上传的文件的原始 url 名称
- r - 如何有效地将巨大的(2L 记录)Oracle DB clob 字段数据提取到 R 数据帧中?
- java - 用于自定义文件类型的 Android 意图过滤器