Resetting simulation

asked 2021-05-27 16:06:02 -0500

PGTKing
13 ●16 ●28 ●38

updated 2021-05-27 17:52:01 -0500

allenh1

3045 ●62 ●89 ●110 http://allen-software.com/

trying to stop program, reset simulation and restart program. Program not responding.

#!/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
import sys

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

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

    def give_force(self):
        self.force = random.choice([-3,3])  # 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)
            #sys.exit("restarting simulation")
            '''
            reset_simulation()
            self.rate.sleep()
            RobotControl()
            '''

        else:
            pass

        print(reward)
        #return reward

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



    def robotcontrolloop():
        if RobotControl() is not None:
            reset_simulation()


    robotcontrolloop()



if __name__ == "__main__":
    # Instantiate the class
    RobotControl()

answered 2021-05-28 10:02:26 -0500

allenh1

3045 ●62 ●89 ●110 http://allen-software.com/

updated 2021-05-28 10:03:28 -0500

So I see a few issues here.

I don't see any rospy.spin() or rospy.spinOnce() calls. In order for your joint_callback to be processed, you need to spin the node.

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

This is one place to add that.

Second, I don't see a definition of reset_simulation() in the code you provided. You'll need to write a function that invokes your service there.

def robotcontrolloop():
    if RobotControl() is not None:
        reset_simulation()

In this function, you construct a RobotControl() instance, check it is not None, and call the reset_simulation function. Since you have an infinite loop in the constructor, the reset_simulation() function will never be called.

I would double check the layout of your functions. The flow of the program seems quite unclear to me. Hopefully the spin call fixes whatever issues you were having, but, if not, please update your question with the result, and a more clear explanation of what you are expecting to achieve with this node.

edit flag offensive delete link

Comments

@allenh1, hey appreciate you looking into my my problem. This issue I am having with the code after my robot achieves the reward and the simulation is reset is the robot stops printing the reward to the terminal and even when it looks like the the robot has appeared to achieve the desired outcome again the simulation doesn't reset. Here is my code.

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)
    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)
    self.reward = None

PGTKing ( 2021-06-02 16:16:25 -0500 )edit

self.force = None self.joint = None self.rate = rospy.Rate(10) self.reward = None

    while self.reward is None:
        self.give_force()
        self.rate.sleep()
        if self.reward is not None:
            reset_simulation()
            self.rate.sleep()
            RobotControl()
        #rospy.spin()




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

    '''
    if self.observation_check() is True:
        self.force = 0
    else:
        pass
    '''


def joint_callback(self, data):
    self.joint = data.position[0]
    #self.reward = None
    if self.observation_check() is True:
        self.reward = (time.time() - start_time)
        #sys.exit("restarting simulation")

PGTKing ( 2021-06-02 16:17:04 -0500 )edit

else: pass

    print(self.reward)
    #return reward

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

'''    
def robotcontrolloop():
    if RobotControl() is not None:
        reset_simulation()
'''

if __name__ == "__main__": # Instantiate the class RobotControl()

PGTKing ( 2021-06-02 16:17:21 -0500 )edit

add a comment

1 Answer

Comments

Question Tools

Stats

Related questions

Resetting simulation edit

1 Answer

Comments

Question Tools

Stats

Related questions

Resetting simulation