ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

@jayess is correct on all points. You set self.force = random.choice([-1,1]) only once, and you generally don't need to call subscriber callbacks directly. The error is from your (unnecessary) direct call, while the output is from the subscriber calls. Furthermore, you never actually call give_force(), so it's not publishing self.force. Judging from your code, you'd like your node to publish a random force continuously, then print both the current force and position when it receives a new JointState message. There are other ways to lay out a node class, but here's an example:

class RobotControl():
    def __init__(self):
        rospy.init_node('robot_control_node', anonymous=True)
        self.talker = rospy.Publisher('/rrbot/joint1_position_controller/command', Float64, queue_size=10)
        self.sub = rospy.Subscriber('/rrbot/joint_states', JointState, self.callback)
        self.force = None  # Generally, it's good to initialize member variables.  What if callback() is called before give_force()?

        rate = rospy.Rate(1)
        while not rospy.is_shutdown():
            self.give_force()
            rate.sleep()

    def give_force(self):
        self.force = random.choice([-1,1])  # Changes force every time, right before publishing
        self.talker.publish(self.force)

    def callback(self,data):
        print(data.position[0])
        print(self.force)  # Prints value defined elsewhere

if __name__=="__main__":
    r = RobotControl()

As an aside, callback() prints the _current_ force value when it's called, so the printed force and position don't have a guaranteed temporal relationship. There are ways to deal with that, but that may be outside the scope here.