When I reset simulation, program doesn't return reward
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()