Ask Your Question
0

ROS node listen for messages while executing instance function

asked 2016-10-18 12:40:46 -0600

updated 2016-10-18 12:41:30 -0600

I have a ROS node that is listening for messages on a topic, and when it receives a 'START' message it makes a call to an instance function of another class JointRecorder.record_playback(). What I want to happen is for the listener to continue listening for a 'STOP' message and execute the instance function concurrently, so that when the 'STOP' message is received it can terminate execution of the instance function. But, right now it just makes a synchronous call to the instance function and waits for it to complete, so the listener never receives the 'STOP' message. How can I have it both listen and execute the instance function?

class JointTrajectoryRecorder:
    def __init__(self, limb, rate, filename):
        self.limb = limb
        self.rate = rate
        self.filename = filename
        self.pub = rospy.Publisher('playback_cmds', String, queue_size=10)
        rospy.init_node('joint_trajectory_recorder', anonymous=True)
        rospy.Subscriber('record_cmds', String, self.respond)
        self.recorder = JointRecorder(self.limb, self.rate)

    def respond(self, cmd):
        # not sure if it will take command while still recording
        if cmd.data == _START_REC:
            self.recorder.record_playback()
        elif cmd.data == _STOP_REC:
            self.recorder.stop()
        elif cmd.data == _DONE:
            self.recorder.persist_data(self.filename)
        else:
            print "Command unknown:", cmd.data

    def run(self):
        print("Getting robot state... ")
        rs = baxter_interface.RobotEnable(CHECK_VERSION)
        print("Enabling robot... ")
        rs.enable()
        print("Running. Ctrl-c to quit")


class JointRecorder:
    def __init__(self, limb, rate):
        self.limb = limb
        self.rate = rospy.Rate(rate)
        self.start_time = rospy.get_time()
        self.arm = baxter_interface.Limb(self.limb)
        self.nominal = Trajectory(self.arm.joint_names(), self.rate)
        self.recordings = []
        self.nav = baxter_interface.Navigator(self.limb)
        self.is_recording = False
        self.is_done = False

    def time_stamp(self):
        return rospy.get_time() - self.start_time

    def stop(self, button_value=True):
        self.is_done = True

    def done(self):
        if rospy.is_shutdown():
            self.stop()
        return self.is_done

    def set_recording(self, button_value):
        self.is_recording = True

    def record_playback(self):
        print "\nRecording playback trajectory..."
        self.is_done = False
        t = Trajectory(self.arm.joint_names(), self.rate)
        while not self.done():
            t.add_timestep(self.time_stamp())
            for name in self.arm.joint_names():
                t.add_position(name, self.arm.joint_angle(name)) 
            self.rate.sleep()
        self.recordings.append(t)
        print "Playback recording complete."
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2016-10-23 17:20:00 -0600

Girts L gravatar image

You will have to return from the callback to receive another callback on the next message. So, the work loop must be running in another tread, maybe in the main thread. It can be as simple as something like:

  • in the callback just save the received message to some variable
  • in the main loop wait for it to become _START_REC, then do your stuff while checking for it to become _STOP_REC
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2016-10-18 12:40:46 -0600

Seen: 66 times

Last updated: Oct 23 '16