def __init__(self):
self.goal_x = 0
self.goal_y = 0
self.heading = 0
self.initGoal = True
self.get_goalbox = False
self.position = Pose()
self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
self.sub_odom = rospy.Subscriber('odom', Odometry, self.getOdometry)
self.reset_proxy = rospy.ServiceProxy('gazebo/reset_simulation', Empty)
self.unpause_proxy = rospy.ServiceProxy('gazebo/unpause_physics', Empty)
self.pause_proxy = rospy.ServiceProxy('gazebo/pause_physics', Empty)
self.respawn_goal = Respawn()
self.past_distance = 0.
self.stopped = 0
#Keys CTRL + c will stop script
rospy.on_shutdown(self.shutdown)
class Env():
def __init__(self, linear_velocity=0.15):
self.goal_x = 0
self.goal_y = 0
self.heading = 0
self.initGoal = True
self.get_goalbox = False
self.position = Pose()
self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
self.sub_odom = rospy.Subscriber('odom', Odometry, self.getOdometry)
self.reset_proxy = rospy.ServiceProxy('gazebo/reset_simulation', Empty)
self.unpause_proxy = rospy.ServiceProxy('gazebo/unpause_physics',
Empty)
self.pause_proxy = rospy.ServiceProxy('gazebo/pause_physics', Empty)
self.respawn_goal = Respawn()
self.past_distance = 0.
self.stopped = 0
self.linear_velocity = linear_velocity
#Keys CTRL + c will stop script
rospy.on_shutdown(self.shutdown)
def shutdown(self):
#---you can stop turtlebot by publishing an empty Twist message---#
rospy.loginfo("Stopping TurtleBot")
self.pub_cmd_vel.publish(Twist())
rospy.sleep(1)
def getGoalDistance(self):
goal_distance = round(
math.hypot(self.goal_x - self.position.x,
self.goal_y - self.position.y), 2)
self.past_distance = goal_distance
return goal_distance
def getOdometry(self, odom):
self.past_position = copy.deepcopy(self.position)
self.position = odom.pose.pose.position
orientation = odom.pose.pose.orientation
orientation_list = [
orientation.x, orientation.y, orientation.z, orientation.w
]
_, _, yaw = euler_from_quaternion(orientation_list)
goal_angle = math.atan2(self.goal_y - self.position.y,
self.goal_x - self.position.x)
heading = goal_angle - yaw
if heading > pi:
heading -= 2 * pi
elif heading < -pi:
heading += 2 * pi
self.heading = round(heading, 3)
def getState(self, scan):
scan_range = []
heading = self.heading
min_range = 0.135
done = False
for i in range(len(scan.ranges)):
if scan.ranges[i] == float('Inf'):
scan_range.append(3.5)
elif np.isnan(scan.ranges[i]):
scan_range.append(0)
else:
scan_range.append(scan.ranges[i])
if min_range > min(scan_range) > 0:
done = True
current_distance = round(
math.hypot(self.goal_x - self.position.x,
self.goal_y - self.position.y), 2)
if current_distance < 0.2:
self.get_goalbox = True
return scan_range + [heading, current_distance], done
def setReward(self, state, done):
current_distance = state[-1]
heading = state[-2]
distance_rate = (self.past_distance - current_distance)
if distance_rate > 0:
# reward = 200.*distance_rate
reward = 1.
if distance_rate <= 0:
# reward = -8.
reward = 0.
self.past_distance = current_distance
a, b, c, d = float('{0:.3f}'.format(self.position.x)), float(
'{0:.3f}'.format(self.past_position.x)), float('{0:.3f}'.format(
self.position.y)), float('{0:.3f}'.format(
self.past_position.y))
if a == b and c == d:
self.stopped += 1
if self.stopped == 20:
rospy.loginfo('Robot is in the same 20 times in a row')
self.stopped = 0
done = True
else:
self.stopped = 0
if done:
rospy.loginfo("Collision!!")
# reward = -550.
reward = -10.
self.pub_cmd_vel.publish(Twist())
if self.get_goalbox:
rospy.loginfo("Goal!!")
# reward = 500.
reward = 100.
self.pub_cmd_vel.publish(Twist())
if world:
self.goal_x, self.goal_y = self.respawn_goal.getPosition(
True, delete=True, running=True)
if target_not_movable:
self.reset()
else:
self.goal_x, self.goal_y = self.respawn_goal.getPosition(
True, delete=True)
self.goal_distance = self.getGoalDistance()
self.get_goalbox = False
return reward, done
def step(self, action):
ang_vel = action
vel_cmd = Twist()
vel_cmd.linear.x = self.linear_velocity
vel_cmd.angular.z = ang_vel
# print('action: ' + str(vel_cmd.linear.x) + ' m/s and ' + str(vel_cmd.angular.z) + ' rad/s')
self.pub_cmd_vel.publish(vel_cmd)
data = None
while data is None:
try:
data = rospy.wait_for_message('scan', LaserScan, timeout=5)
except:
pass
state, done = self.getState(data)
reward, done = self.setReward(state, done)
return np.asarray(state), reward, done
def reset(self):
rospy.wait_for_service('gazebo/reset_simulation')
try:
self.reset_proxy()
except (rospy.ServiceException) as e:
print("gazebo/reset_simulation service call failed")
data = None
while data is None:
try:
data = rospy.wait_for_message('scan', LaserScan, timeout=5)
except:
pass
if self.initGoal:
self.goal_x, self.goal_y = self.respawn_goal.getPosition()
self.initGoal = False
else:
self.goal_x, self.goal_y = self.respawn_goal.getPosition(
True, delete=True)
self.goal_distance = self.getGoalDistance()
state, _ = self.getState(data)
return np.asarray(state)