How can I do: subscribe to the data listened from a publisher, make changes and publish it again? [-CodePudding

First, I want to capture pose values by subscribing to teleop_key from a turtle. Then I want to change these captured values and publish to a second turtle. The problem is that I couldn't capture the pose values as a global variables. And due to this I couldn't change the variables and published the modified ones.

I think I have an almost finished code. That's why I'm going to throw them all out directly.

#!/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()

And I did the connection between turtle1 and turtle2 in the lunch file below:

<?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>

And lastly here my package.xml code:

<?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="[email protected]">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>

Not: I work in catkin workspace the mistake couldn't be here because I run many different code without trouble

CodePudding user response：

As pointed out by one of the commenters, you need to declare you pos values are global inside your callback function. In Python variables must be declared global within the scope they are going to be used; i.e. function scope. When this doesn't happen, the interpreter doesn't know you to use global variables and simple creates a local variable. Note that this is only for assignment operations, so it does not need to be done when you get ready to publish. Take the following example:

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

As another note, this will most likely break since the global variables will not always be assigned before trying to be used. So you should assign them at the very top of the file. Finally, you should not be declaring a subscriber in the main run loop. It should be done once right after node_init.

CodePudding user response：

It is done !!!

#!/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()