我该怎么办:订阅从发布者处收听的数据,进行更改并再次发布?[ ROS - Python]

pxiryf3j  于 2022-11-19  发布在  Python
关注(0)|答案(2)|浏览(157)

首先,我想通过从一个turtle订阅teleop_key来捕获pose值。然后我想更改这些捕获的值并发布到另一个turtle。问题是我无法将pose值捕获为全局变量。因此,我无法更改变量并发布修改后的变量。
我想我已经有一个几乎完成的代码了。这就是为什么我打算直接把它们都扔出去。

#!/usr/bin/env python3
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
import rospy as rp

global pos_l_x,pos_l_y,pos_l_z,pos_a_x,pos_a_y,pos_a_z 

def pose_callback(msg):
    rp.loginfo("("+ str(msg.x) + "," + str(msg.y) + "," + str(msg.theta)+ ")")

    pos_l_x = msg.x
    pos_l_y = msg.y
    pos_a_z = msg.theta

if __name__ == '__main__':
    rp.init_node("turtle_inverse")

    while not rp.is_shutdown():
                
        sub = rp.Subscriber("/turtlesim1/turtle1/pose", Pose, callback= pose_callback)
        rate = rp.Rate(1)
        rp.loginfo("Node has been started")
        
        cmd = Twist()
    
        cmd.linear.x = -1*pos_l_x
        cmd.linear.y = -1*pos_l_y
        cmd.linear.z = 0
        cmd.angular.x = 0
        cmd.angular.y = 0
        cmd.angular.z = -1*pos_a_z
        
        pub = rp.Publisher("/turtlesim2/turtle1/cmd_vel", Twist, queue_size=10)
        try:
            pub.publish(cmd)
        except rp.ServiceException as e:
            rp.logwarn(e)

        rate.sleep()

    rp.spin()

我在下面的午餐文件中做了turtle1和turtle2之间的连接:

<?xml version="1.0"?>
<launch>

    <group ns="turtlesim1">
        <node pkg="turtlesim" type="turtlesim_node" name="turtle1">
            <remap from="/turtle1/cmd_vel" to="vel_1"/>
        </node>
        <node pkg="turtlesim" type="turtle_teleop_key" name="Joyistic" output= "screen">
            <remap from="/turtle1/cmd_vel" to="vel_1"/>
        </node>
    </group>

    <group ns="turtlesim2">
        <node pkg="turtlesim" type="turtlesim_node" name="turtle1">
        </node>
    </group>

    <node pkg="turtlesim" type="mimic" name="mimic">
        <remap from="input" to="turtlesim1/turtle1"/>
        <remap from="output" to="turtlesim2/turtle1"/>
    </node>

</launch>

最后是我的package.xml代码:

<?xml version="1.0"?>
<package format="2">
  <name>my_robot_controller</name>
  <version>0.0.0</version>
  <description>The my_robot_controller package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="(I delete it for sharing)">enes</maintainer>

  <!-- One license tag required, multiple allowed, one license per tag -->
  <!-- Commonly used license strings: -->
  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  
  <build_depend>rospy</build_depend>
  <build_depend>turtlesim</build_depend>
  <build_depend>geometry_msgs</build_depend>
  
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>turtlesim</build_export_depend>
  <build_export_depend>geometry_msgs</build_export_depend>
  
  <exec_depend>rospy</exec_depend>
  <exec_depend>turtlesim</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>

  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

不是:我在柳絮工作区工作,错误不可能在这里,因为我运行许多不同的代码没有麻烦

s6fujrry

s6fujrry1#

正如一位评论者指出的,你需要声明你的pos值是globalinside 你的回调函数。也就是函数作用域。如果没有发生这种情况,解释器就不知道你使用了全局变量,而只是创建了一个局部变量。注意,这只适用于赋值操作,所以当你准备发布时不需要这样做。举下面的例子:

def pose_callback(msg):
    rp.loginfo("("+ str(msg.x) + "," + str(msg.y) + "," + str(msg.theta)+ ")")

    global pos_l_x, pos_l_y, pos_a_z

    pos_l_x = msg.x
    pos_l_y = msg.y
    pos_a_z = msg.theta

另一个注意事项是,这很可能会中断,因为全局变量在尝试使用之前并不总是被赋值的。所以你应该在文件的最顶部赋值它们。最后,你 * 不 * 应该在主运行循环中声明一个订阅者。它应该在node_init之后做一次。

flvlnr44

flvlnr442#

这是做!!!

#!/usr/bin/env python3
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
import rospy as rp

pos_l_x,pos_l_y,pos_l_z,pos_a_x,pos_a_y,pos_a_z = 0,0,0,0,0,0 

def pose_callback(msg):
    rp.loginfo("("+ str(msg.linear.x) + "," + str(msg.linear.y) + "," + str(msg.angular.z)+ ")")
    global pos_l_x,pos_l_y,pos_l_z,pos_a_x,pos_a_y,pos_a_z 
    pos_l_x = msg.linear.x
    pos_l_y = msg.linear.y
    pos_l_z = msg.linear.z
    pos_a_x = msg.angular.x
    pos_a_y = msg.angular.y
    pos_a_z = msg.angular.z

if __name__ == '__main__':
    rp.init_node("turtle_inverse")
    sub = rp.Subscriber("/turtlesim1/turtle1/cmd_vel", Twist, callback= pose_callback)
    rate = rp.Rate(1)
    rp.loginfo("Node has been started")

    while not rp.is_shutdown():
        cmd = Twist()

        cmd.linear.x = -1*pos_l_x
        cmd.linear.y = -1*pos_l_y
        cmd.linear.z = -1*pos_l_z
        cmd.angular.x = -1*pos_a_x
        cmd.angular.y = -1*pos_a_y
        cmd.angular.z = -1*pos_a_z
    
        pub = rp.Publisher("/turtlesim2/turtle1/cmd_vel", Twist, queue_size=10)
        try:
            pub.publish(cmd)
        except rp.ServiceException as e:
            pass
        pos_l_x,pos_l_y,pos_l_z,pos_a_x,pos_a_y,pos_a_z = 0,0,0,0,0,0
    rate.sleep()
rp.spin()

相关问题