When I reset simulation, program doesn't return reward

asked 2021-03-18 13:04:40 -0500

PGTKing gravatar image

updated 2021-03-19 16:04:47 -0500

tryan gravatar image

I am using rospy.ServiceProxy('/gazebo/reset_simulation', Empty) to reset the simulation. But if I have this line of code, the simulation resets and the robot still takes commands but the terminal is not returning the reward.

here is my code:

#!/usr/bin/env python3

import rospy


from std_msgs.msg import Float64

import math

import random

from sensor_msgs.msg import JointState

import numpy as np

import time

import threading

from std_srvs.srv import Empty

start_time = time.time()
reset_simulation = rospy.ServiceProxy('/gazebo/reset_simulation', Empty)

class RobotControl:

    def __init__(self):
        rospy.init_node('robot_control_node', anonymous=True)
        #reset_simulation()
        self.talker = rospy.Publisher('/rrbot/joint1_position_controller/command', Float64, queue_size=10)
        rospy.Subscriber('/rrbot/joint_states', JointState, self.joint_callback)
        self.force = None
        self.joint = None
        self.rate = rospy.Rate(10)
        #HERE EITHER USE THE THREAD if you do not want the code to be stuck
        self.pub_thread = threading.Thread(target=self.publisher)
        # start separate entity publishing data
        self.pub_thread.start()
        # SPIN SINCE WE DO NOT NEED TO DO ANYTHING 
        rospy.spin()

    def publisher(self):
        while not rospy.is_shutdown():
            self.give_force()
            self.rate.sleep()

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

    def joint_callback(self, data):
        self.joint = data.position[0]
        reward = None
        if self.observation_check() is True:
            reward = (time.time() - start_time)
            reset_simulation()
        else:
            pass

        print(reward)

    def observation_check(self):
        max_height = 1.5
        min_height = -1.5
        if min_height < self.joint < max_height:
            done = True
        else:
            done = False
        return done


if __name__ == "__main__":
    # Instantiate the class
    my_robot = RobotControl()
edit retag flag offensive close merge delete