def set_model_pose(self, pose, twist=None, model='sub8'):
'''
Set the position of 'model' to 'pose'.
It may be helpful to kill the sub before moving it.
TODO:
- Deprecate kill stuff
'''
set_state = yield self.nh.get_service_client('/gazebo/set_model_state', SetModelState)
if twist is None:
twist = numpy_to_twist([0, 0, 0], [0, 0, 0])
model_state = ModelState()
model_state.model_name = model
model_state.pose = pose
model_state.twist = twist
if model == 'sub8':
# TODO: Deprecate kill stuff (Zach's PR upcoming)
kill = self.nh.get_service_client('/set_kill', SetKill)
yield kill(SetKillRequest(kill=Kill(id='initial', active=False)))
yield kill(SetKillRequest(kill=Kill(active=True)))
yield self.nh.sleep(.1)
yield set_state(SetModelStateRequest(model_state))
yield self.nh.sleep(.1)
yield kill(SetKillRequest(kill=Kill(active=False)))
else:
set_state(SetModelStateRequest(model_state))
def launch_torpedo(self, srv):
'''
Find position of sub and launch the torpedo from there.
TODO:
- Test to make sure it always fires from the right spot in the right direction.
(It seems to but I haven't tested from all rotations.)
'''
sub_state = self.get_model(model_name='sub8')
sub_pose = msg_helpers.pose_to_numpy(sub_state.pose)
# Translate torpedo init velocity so that it first out of the front of the sub.
muzzle_vel = np.array([10, 0, 0])
v = geometry_helpers.rotate_vect_by_quat(np.append(muzzle_vel, 0), sub_pose[1])
launch_twist = Twist()
launch_twist.linear.x = v[0]
launch_twist.linear.y = v[1]
launch_twist.linear.z = v[2]
# This is offset so it fires approx at the torpedo launcher location.
launch_pos = geometry_helpers.rotate_vect_by_quat(np.array([.4, -.15, -.3, 0]), sub_pose[1])
model_state = ModelState()
model_state.model_name = 'torpedo'
model_state.pose = msg_helpers.numpy_quat_pair_to_pose(sub_pose[0] + launch_pos, sub_pose[1])
model_state.twist = launch_twist
self.set_torpedo(model_state)
self.launched = True
return True
def publish(self):
gaz = ModelState()
gaz.model_name = self.model_name
gaz.pose = self.pose
gaz.twist = self.vel
self.pub.publish(gaz)
rospy.loginfo(gaz)
self.pose, self.vel = None, None
def _cb_linkdata(self, msg):
for pos, item in enumerate(msg.name):
twist = msg.twist[pos]
pose = msg.pose[pos]
self.model_twists[item] = twist
self.model_poses[item] = pose
ms = ModelState()
ms.pose = pose
ms.twist = twist
ms.model_name = item
self.model_states[item] = ms
def moving(self):
while not rospy.is_shutdown():
# When training in the moving_obstacles module
while True:
gate = ModelState()
model = rospy.wait_for_message('gazebo/model_states',
ModelStates)
for i in range(len(model.name)):
if model.name[i] == 'gate_moving' or model.name[
i] == 'gate_moving_1':
gate.model_name = model.name[i]
gate.pose = model.pose[i]
gate.twist = Twist()
gate.twist.linear.z = GATE_MOVE_SPEED * self.gate_dir
# Gate is moving down and close to bottom, wait "shut"
if (model.pose[i].position.z < 0.3
and self.gate_dir == -1):
self.gate_dir = 1
gate.twist.linear.z = 0
gate.pose.position.z = 0.25
self.pub_model.publish(gate)
try:
rospy.sleep(GATE_WAIT_TIME)
except rospy.exceptions.ROSTimeMovedBackwardsException, e:
print(
"Ros error due to reset during sleep, disregard"
)
continue
# Gate is moving up and close to top, wait "open"
if (model.pose[i].position.z > 1
and self.gate_dir == 1):
self.gate_dir = -1
gate.twist.linear.z = 0
gate.pose.position.z = 1
self.pub_model.publish(gate)
try:
rospy.sleep(GATE_WAIT_TIME)
except rospy.exceptions.ROSTimeMovedBackwardsException, e:
print(
"Ros error due to reset during sleep, disregard"
)
continue
self.pub_model.publish(gate)
try:
rospy.sleep(0.1)
except rospy.exceptions.ROSTimeMovedBackwardsException, e:
print(
"Ros error due to reset during sleep, disregard"
)
def moving_y(self, distance):
obstacle = ModelState()
model = rospy.wait_for_message('gazebo/model_states', ModelStates)
model_original_pose_y = model.pose[0].position.y
for i in range(len(model.name)):
if model.name[i] == self.model_name and round((model.pose[i].position.y),1) <= round(self.y_model_pose,1)+1.0 and self.flag1 == 0:
obstacle.model_name = self.model_name
obstacle.pose = model.pose[i]
obstacle.twist = Twist()
obstacle.twist.linear.y = distance
obstacle.twist.angular.z = 0
self.pub_model.publish(obstacle)
elif model.name[i] == self.model_name and round(model.pose[i].position.y,1) != round(self.y_model_pose,1):
self.flag1 = 1
obstacle.model_name = self.model_name
obstacle.pose = model.pose[i]
obstacle.twist = Twist()
obstacle.twist.linear.y = -distance
obstacle.twist.angular.z = 0
self.pub_model.publish(obstacle)
elif round(model.pose[i].position.y,1) == round(self.y_model_pose,1):
self.flag1 = 0
def _reset(self):
# print("Resetting ...")
rospy.wait_for_service('gazebo/set_model_state')
try:
state = ModelState()
state.model_name = self.name_model
state.reference_frame = "world"
state.pose = self.turtlebot_state.pose
state.twist = self.turtlebot_state.twist
self.set_model(state)
except rospy.ServiceException, e:
print("/gazebo/set_model_state service call failed")
def callback(data):
twist = Twist()
twist.linear.x = data.linear.x
twist.linear.y = data.linear.y
twist.linear.z = data.linear.z
twist.angular.x = data.angular.x
twist.angular.y = data.angular.y
twist.angular.z = data.angular.z
state = ModelState()
state.model_name = "youbot_2"
state.twist = twist
state.reference_frame = "youbot_2"
ret = set_state(state)
def moving_tile_y_alternate_long_path(self):
obstacle = ModelState()
model = rospy.wait_for_message('gazebo/model_states', ModelStates)
model_original_pose_y = model.pose[0].position.y
for i in range(len(model.name)):
if model.name[i] == self.model_name and round(model.pose[i].position.y) <= round(self.y_model_pose)+6.0 and round(model.pose[i].position.y) >= -round(self.y_model_pose)+2. and self.flag1 == 0:
obstacle.model_name = self.model_name
obstacle.pose = model.pose[i]
obstacle.twist = Twist()
obstacle.twist.linear.y = -1.
obstacle.twist.angular.z = 0
self.pub_model.publish(obstacle)
elif model.name[i] == self.model_name and round(model.pose[i].position.y) != round(self.y_model_pose):
self.flag1 = 1
obstacle.model_name = self.model_name
obstacle.pose = model.pose[i]
obstacle.twist = Twist()
obstacle.twist.linear.y = 1.
obstacle.twist.angular.z = 0
self.pub_model.publish(obstacle)
elif round(model.pose[i].position.y) == round(self.y_model_pose):
self.flag1 = 0
def modelStatesCb(self, msg):
'''
Callback for gazebo model states
@param msg: received message
@type msg: gazebo_msgs/ModelStates
'''
for i in range(len(msg.name)):
if msg.name[i] in self._gazebo_robots:
model_state = ModelState()
model_state.model_name = msg.name[i]
model_state.pose = msg.pose[i]
model_state.twist = msg.twist[i]
self._gazebo_robots[msg.name[i]]['model'].update(
model_state, rospy.Time.now())
elif msg.name[i] in self._gazebo_objects:
model_state = ModelState()
model_state.model_name = msg.name[i]
model_state.pose = msg.pose[i]
model_state.twist = msg.twist[i]
self._gazebo_objects[msg.name[i]]['model'].update(
model_state, rospy.Time.now())
def main():
rospy.init_node('set_vel')
current_time = rospy.Time.now().to_sec()
last_time = rospy.Time.now().to_sec()
x = 5
y = 0
theta = 0
rate = rospy.Rate(5)
rospy.Subscriber('cmd_velBox', TwistStamped, callback)
while not rospy.is_shutdown():
current_time = rospy.Time.now().to_sec()
dt = (current_time - last_time)
delta_x = lin_x.data * dt
delta_y = lin_y.data * dt
delta_theta = ang_z.data * dt
x += delta_x
y += delta_y
theta += delta_theta
q = theta_to_quaternion(theta)
modelstate = ModelState()
modelstate.model_name = 'simple_box'
modelstate.reference_frame = "world"
model_twist = Twist()
model_twist.linear.x = 0
model_twist.linear.y = 0
model_twist.linear.z = 0
model_twist.angular.x = 0
model_twist.angular.y = 0.0
model_twist.angular.z = 0.0
model_pose = Pose()
model_pose.position.x = x
model_pose.position.y = y
model_pose.position.z = 0
model_pose.orientation.x = q.x
model_pose.orientation.y = q.y
model_pose.orientation.z = q.z
model_pose.orientation.w = q.w
modelstate.twist = model_twist
modelstate.pose = model_pose
last_time = rospy.Time.now().to_sec()
rate.sleep()
rospy.wait_for_service('/gazebo/set_model_state')
try:
set_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
resp = set_state(modelstate)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def try_to_pick_up_apple(self):
"""Try to pickup apple."""
is_apple_picked = False # idicate wether picked apple or not
try:
distance_min, number_min = self.closest_apple()
rospy.logdebug(
"Trying to pick up apple - distance %.2f, "
"minimum to succeed %.2f", distance_min, self.apple_distance)
if distance_min <= self.apple_distance and self.apple_idx_list:
# Instead of deleting apples from world we change it's position
# by setting them far away from environment
new_model_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
model_state = ModelState()
apple_name = 'cricket_ball_' + str(number_min)
rospy.loginfo('Picked %s', apple_name)
model_state.model_name = apple_name
twist = Twist()
twist = self._set_twist(twist)
model_state.twist = twist
pose = Pose()
pose.position.x = 99
pose.position.y = -99
pose.position.z = 0.0
pose.orientation.x = 0.0
pose.orientation.y = 0.0
pose.orientation.z = 0.0
pose.orientation.w = 0.0
model_state.pose = pose
model_state.reference_frame = 'world'
new_model_state(model_state)
# We need to keep up a list of pick-up apples because we dont actually delete
# instead change position but we dont want to try pick them up again
self.apple_idx_list.remove(number_min)
is_apple_picked = True
else:
rospy.logdebug(
'Come closer to the apple. Min distance is %.2f',
self.apple_distance)
except rospy.ServiceException as err:
rospy.logerr("Get Model State service call failed: %s", err)
return is_apple_picked
def talker():
pub = rospy.Publisher('/ackermann_cmd', Float32 speed, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
new_model_state_msg = ModelState()
new_model_state_msg.model_name = ackermann_model_name
new_model_state_msg.pose = ackermann_pose
new_model_state_msg.twist = ackermann_twist
publisher.publish(new_model_state_msg)
def moving(self):
while not rospy.is_shutdown():
model = rospy.wait_for_message('gazebo/model_states', ModelStates)
for i in range(len(model.name)):
if model.name[i] == 'obstacle_1':
obstacle_1 = ModelState()
obstacle_1.model_name = model.name[i]
obstacle_1.pose = model.pose[i]
obstacle_1.twist = model.twist[i]
obstacle_1.pose.position.x = random(-20, 20) / 10
obstacle_1.twist.linear.x = random(-20, 20) / 10
self.pub_model.publish(obstacle_1)
def reset_simulation(self):
# Reset the joints
new_angles = {}
new_angles['j_ankle1_l'] = 0.0
new_angles['j_ankle1_r'] = 0.0
new_angles['j_ankle2_l'] = 0.0
new_angles['j_ankle2_r'] = 0.0
new_angles['j_gripper_l'] = 0.0
new_angles['j_gripper_r'] = 0.0
new_angles['j_high_arm_l'] = 1.50 # Arm by the side of the body
new_angles['j_high_arm_r'] = 1.50 # Arm by the side of the body
new_angles['j_low_arm_l'] = 0.0
new_angles['j_low_arm_r'] = 0.0
new_angles['j_pan'] = 0.0
new_angles['j_pelvis_l'] = 0.0
new_angles['j_pelvis_r'] = 0.0
new_angles['j_shoulder_l'] = 0.0
new_angles['j_shoulder_r'] = 0.0
new_angles['j_thigh1_l'] = 0.0
new_angles['j_thigh1_r'] = 0.0
new_angles['j_thigh2_l'] = 0.0
new_angles['j_thigh2_r'] = 0.0
new_angles['j_tibia_l'] = 0.0
new_angles['j_tibia_r'] = 0.0
new_angles['j_tilt'] = -0.262 # Tilt the head forward a little
new_angles['j_wrist_l'] = 0.0
new_angles['j_wrist_r'] = 0.0
# Default delay of setting angles is 2 seconds
self.darwin.set_angles_slow(new_angles)
rospy.sleep(5)
# Reset the robot position
# Send model to x=0,y=0
pose = Pose()
pose.position.z = 0.31
twist = Twist()
md_state = ModelState()
md_state.model_name = self.model_name
md_state.pose = pose
md_state.twist = twist
md_state.reference_frame = self.relative_entity_name
# Service call to reset model
try:
response = self.set_model_state(md_state)
except rospy.ServiceException, e:
self.log_write("Darwin model state initialization service call failed: " + str(e))
def reset_simulation():
global current_model_x, current_model_y, current_model_z, has_fallen
# Reset the joints
new_angles = {}
new_angles['j_ankle1_l'] = 0.0
new_angles['j_ankle1_r'] = 0.0
new_angles['j_ankle2_l'] = 0.0
new_angles['j_ankle2_r'] = 0.0
new_angles['j_gripper_l'] = 0.0
new_angles['j_gripper_r'] = 0.0
new_angles['j_high_arm_l'] = 1.50 # Arm by the side of the body
new_angles['j_high_arm_r'] = 1.50 # Arm by the side of the body
new_angles['j_low_arm_l'] = 0.0
new_angles['j_low_arm_r'] = 0.0
new_angles['j_pan'] = 0.0
new_angles['j_pelvis_l'] = 0.0
new_angles['j_pelvis_r'] = 0.0
new_angles['j_shoulder_l'] = 0.0
new_angles['j_shoulder_r'] = 0.0
new_angles['j_thigh1_l'] = 0.0
new_angles['j_thigh1_r'] = 0.0
new_angles['j_thigh2_l'] = 0.0
new_angles['j_thigh2_r'] = 0.0
new_angles['j_tibia_l'] = 0.0
new_angles['j_tibia_r'] = 0.0
new_angles['j_tilt'] = 0.0
new_angles['j_wrist_l'] = 0.0
new_angles['j_wrist_r'] = 0.0
# Default delay of setting angles is 2 seconds
darwin.set_angles_slow(new_angles)
# Reset the robot position
# Send model to x=0,y=0
pose = Pose()
pose.position.z = 0.31
twist = Twist()
md_state = ModelState()
md_state.model_name = model_name
md_state.pose = pose
md_state.twist = twist
md_state.reference_frame = 'world'
# Service call to reset model
try:
response = set_model_state(md_state)
except rospy.ServiceException, e:
print "Darwin model state initialization service call failed: %s" % e
def make_a_brave_new_forest(self):
# generate random samples
random_interval_x = self.spacing_trees_x / 3
offset_x = 7.5
random_interval_y = self.spacing_trees_y
offset_y = -int(self.num_trees_y * self.spacing_trees_y / 2) + 3
x = np.linspace(
offset_x, offset_x + (self.num_trees_x - 1) * self.spacing_trees_x,
self.num_trees_x)
y = np.linspace(
offset_y, offset_y + (self.num_trees_y - 1) * self.spacing_trees_y,
self.num_trees_y)
counter = 0
np.random.seed() #use seed from sys time to build new env on reset
for i in range(self.num_trees_x):
for j in range(self.num_trees_y):
name = 'unit_cylinder_' + str(counter)
counter += 1
noise_x = np.random.random() - 0.5
noise_x *= random_interval_x
noise_y = np.random.random() - 0.5
noise_y *= random_interval_y
x_tree = x[i] + noise_x
y_tree = y[j] + noise_y
model_pose = Pose()
model_pose.position.x = x_tree
model_pose.position.y = y_tree
model_pose.position.z = 5.0
model_pose.orientation.x = 0.0
model_pose.orientation.y = 0.0
model_pose.orientation.z = 0.0
model_pose.orientation.w = 1.0
model_twist = Twist()
model_state = ModelState()
model_state.model_name = name
model_state.pose = model_pose
model_state.twist = model_twist
model_state.reference_frame = 'world' # change to 'world'?
rospy.wait_for_service('/gazebo/set_model_state')
try:
self.set_model_state_proxy(model_state)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def call_service(self, req):
"""
Move the turtlebot to the requested pose.
if collision: return turtlebot to original pose, return
"""
current_pose = self.get_current_pose()
desired_angle = np.clip(req.heading.data, -np.pi, np.pi)
desired_distance = np.clip(req.distance.data, 0, 1.5)
self.set_heading_angle(desired_angle)
current_position = self.get_current_position(current_pose)
target_pose = np.zeros((2, ))
target_pose[
0] = current_position[0] + desired_distance * np.cos(desired_angle)
target_pose[
1] = current_position[1] + desired_distance * np.sin(desired_angle)
waypoints_x = np.linspace(current_position[0], target_pose[0],
self.step_size)
waypoints_y = np.linspace(current_position[1], target_pose[1],
self.step_size)
waypoints = np.vstack([waypoints_x, waypoints_y]).T
for i in range(waypoints.shape[0]):
self.set_position(waypoints[i])
rospy.sleep(rospy.Duration(0.1))
scan_data = rospy.wait_for_message("/turtlebot_scan", TurtleBotScan)
model_state = self.get_model_state(model_name="mobile_base")
ground_truth = ModelState()
ground_truth.pose = model_state.pose
ground_truth.twist = model_state.twist
ground_truth.model_name = "mobile_base"
noisy_heading = Float32()
noisy_heading.data = desired_angle + np.random.normal(
0, self.rotation_noise)
noisy_distance = Float32()
noisy_distance.data = desired_distance + np.random.normal(
0, self.translation_noise)
return TurtleBotControlResponse(ground_truth=ground_truth,
noisy_heading=noisy_heading,
noisy_distance=noisy_distance,
scan_data=scan_data)
def _reset(self):
#cv2.destroyAllWindows()
# Resets the state of the environment and returns an initial observation.
# rospy.wait_for_service('/gazebo/reset_simulation')
# try:
# #reset_proxy.call()
# self.reset_proxy()
# except rospy.ServiceException:
# print ("/gazebo/reset_simulation service call failed")
rospy.wait_for_service('gazebo/set_model_state')
state = ModelState()
state.model_name = self.name_model
state.reference_frame = "world"
state.pose= self.turtlebot_state.pose
state.twist = self.turtlebot_state.twist
self.set_model(state)
# self.num_target = np.random.randint(3)
self.checked_point = [0] * self.num_hint
# self.num_hint = len(self.hint_pos[self.num_target])
# self.setTarget()
# Unpause simulation to make observation
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except rospy.ServiceException:
print ("/gazebo/unpause_physics service call failed")
#read laser data
data = None
while data is None:
try:
data = rospy.wait_for_message('/scan', LaserScan, timeout=5)
except:
pass
pos = self.model_state(self.name_model, "world").pose.position
done = self.calculate_observation(data, pos)
image, laser = self.discretize_observation(data)
rospy.wait_for_service('/gazebo/pause_physics')
try:
#resp_pause = pause.call()
self.pause()
except rospy.ServiceException:
print ("/gazebo/pause_physics service call failed")
return np.reshape(np.array([image, np.asarray(laser), np.asarray([pos.x, pos.y], dtype = np.float32)]), [1, 3])
def set_states(**kwargs):
# rospy.init_node('model_state_client')
rospy.wait_for_service('/gazebo/set_model_state')
model_state_service = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
for name, pose in kwargs.items():
model_state = ModelState()
model_state.model_name = name
model_state.pose = Pose(Point(*pose),
Quaternion(*(0.0, 0.0, 0.0, 0.0)))
model_state.twist = Twist(Vector3(*(0.0, 0.0, 0.0)),
Vector3(*(0.0, 0.0, 0.0)))
model_state.reference_frame = ''
model_state_service(model_state)
rospy.sleep(1.0)
def states_callback(self, states_msg):
states = states_msg.data
model_state_msg = ModelState()
model_state_msg.pose = Pose()
model_state_msg.twist = Twist()
model_state_msg.pose.position.x = states[0]
model_state_msg.pose.position.y = states[1]
model_state_msg.pose.position.z = 0
model_state_msg.pose.orientation = Quaternion(*tf.transformations.quaternion_from_euler(0, 0, states[2]))
model_state_msg.model_name = "unicycle"
model_state_msg.reference_frame = "world"
self.pub.publish(model_state_msg)
def moving_tile_y_long_path1(self):
obstacle = ModelState()
model = rospy.wait_for_message('gazebo/model_states', ModelStates)
model_original_pose_y = model.pose[0].position.y
for i in range(len(model.name)):
if model.name[i] == self.model_name and self.flag1==0 and self.flag2 == 1 and round(model.pose[i].position.y) > round(self.y_model_pose):
obstacle.model_name = self.model_name
obstacle.pose = model.pose[i]
obstacle.twist = Twist()
obstacle.twist.linear.y = -speed
obstacle.twist.angular.z = 0
self.pub_model.publish(obstacle)
elif model.name[i] == self.model_name and round(model.pose[i].position.y) <= -3.0:
self.flag3 = 1
self.flag2 = 0
def moveGazeboModels(self, command):
rospy.wait_for_service('/gazebo/set_model_state')
try:
move_model = rospy.ServiceProxy('gazebo/set_model_state',
SetModelState)
for c in command.models:
destModelState = ModelState()
destModelState.model_name = c.name
pose = Pose()
copyRosProto(c.pose, pose)
destModelState.pose = pose
destModelState.twist = Twist()
destModelState.reference_frame = 'world'
resp = move_model(destModelState)
except rospy.ServiceException, e:
rospy.logerr("Set model state service call failed: {0}".format(e))
def talker():
pub = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=100)
sub = rospy.Subscriber('/gazebo/model_states', ModelStates, sub_callback)
rospy.init_node('husky', anonymous=True)
rate = rospy.Rate(10) # 10hz
pub_msg = ModelState()
while not rospy.is_shutdown():
if data_husky.pose:
pub_msg.model_name = 'husky_sim'
pub_msg.pose = data_husky.pose
pub_msg.twist = data_husky.twist
pub_msg.twist.linear.x = pub_msg.twist.linear.x + 0.11
pub.publish(pub_msg)
rate.sleep()
def try_to_pick_up_apple(self):
reward = 0
try:
distanceLim = 0.50 #We should define the minimum distance to apple, where robot can pick up
distanceMin, numberMin = self.closest_apple()
print(
'Trying to pick up apple - distance %.2f, minimum to succeed %.2f'
% (distanceMin, distanceLim))
if distanceMin <= distanceLim:
#delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
new_model_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
model_state = ModelState()
apple_name = 'cricket_ball_' + str(numberMin)
print('Picked %s' % apple_name)
model_state.model_name = apple_name
twist = Twist()
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
model_state.twist = twist
pose = Pose()
pose.position.x = 0.2
pose.position.y = -2.4
pose.position.z = 0.0
pose.orientation.x = 0.0
pose.orientation.y = 0.0
pose.orientation.z = 0.0
pose.orientation.w = 0.0
model_state.pose = pose
model_state.reference_frame = 'world'
#delete_apple = delete_model(apple_name)
new_apple_state = new_model_state(model_state)
reward = 1000
else:
reward = 0
# reward = -1
print(
'You should come closer to the apple. Minimal distance = 0.50'
)
except rospy.ServiceException as e:
rospy.loginfo(
"Get Model State service call failed: {0}".format(e))
return reward
def talker(path):
rospy.init_node('talker', anonymous=True)
pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=10)
rate = rospy.Rate(10) # 10hz
temp = path
x = ModelState()
x.model_name = "piano2"
x.pose = Pose()
x.pose.orientation.y = 0
x.pose.orientation.w = 1.0
x.pose.orientation.x = 0
x.pose.orientation.z = 0
while not rospy.is_shutdown():
x.twist = Twist()
x.twist.linear.y = 0
x.twist.angular.y=0
for i in range(0, len(temp)):
x.pose.position.x = temp[i][0]
x.pose.position.y = temp[i][1]
x.pose.position.z = 0.31
#angle_to_turn = atan2 (temp[i+1][1]-temp[i][1], temp[i+1][0]-temp[i+1][0])*math.pi/180
x.pose.orientation.z = random.uniform(0, 1)
#x.twist = Twist()
#x.twist.linear.x = randint(-60, 60)*math.pi/180
rospy.loginfo(x)
pub.publish(x)
time.sleep(0.3)
# print "one loop done"
exit()
def modelStatesCallback(msg):
global model_name, model_state_pub, x_vel, y_vel, model_state_sub
index_of_interest = -1
for i in range(len(msg.name)):
if msg.name[i] == model_name:
index_of_interest = i
break
if index_of_interest >= 0:
model_state = ModelState()
model_state.model_name = model_name
model_state.pose = msg.pose[index_of_interest]
twist = Twist()
twist.linear.x = x_vel
twist.linear.y = y_vel
if msg.twist[index_of_interest] != twist:
model_state.twist = twist
model_state_pub.publish(model_state)
def reset_model(robot, pose):
model_state = ModelState()
model_state.model_name = robot
model_state.pose = copy.deepcopy(pose)
if randomise_positions:
model_state.pose.position.y += random.random() * 0.5 - 0.25
if model_state.pose.position.x > 0:
model_state.pose.position.x -= random.random() * 0.4 - 0.06
else:
model_state.pose.position.x += random.random() * 0.4 - 0.06
yaw = 2 * math.acos(model_state.pose.orientation.w)
yaw += random.random() * 1.6 - 0.8
model_state.pose.orientation.z = math.sin(yaw/2)
model_state.pose.orientation.w = math.cos(yaw/2)
model_state.twist = Twist()
model_state.reference_frame = "world"
set_robot_pos(model_state)
def set_velocity(x, y):
state_publisher = Publisher("/gazebo/set_model_state",
ModelState,
queue_size=10)
rospy.init_node("model_state_setter", anonymous=True)
new_vel = Twist()
new_vel.linear.x = x
new_vel.linear.y = y
new_state = ModelState()
new_state.twist = new_vel
new_state.model_name = "robot"
new_state.reference_frame = "ground_plane"
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
rospy.loginfo(new_state)
state_publisher.publish(new_state)
rate.sleep()
def setObject():
objName = 'test_robot'
pub = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=10)
rospy.init_node('setObject', anonymous=True)
rate = rospy.Rate(1) # 10hz
while not rospy.is_shutdown():
msg = ModelState()
resp1 = gms_client(objName, objName)
msg = ModelState()
msg.pose = resp1.pose
msg.twist = resp1.twist
msg.pose.position.x += .30
msg.model_name = objName
print msg
pub.publish(msg)
rate.sleep()
print "here"
def set_position(x=0, y=0):
rospy.init_node("set_pos")
curr_state = getModelState()
new_state = ModelState()
new_state.model_name = 'gem'
new_state.pose = curr_state.pose
new_state.pose.position.x = x
new_state.pose.position.y = y
new_state.pose.position.z = 1
q = euler_to_quaternion([np.pi / 2, 0, 0])
new_state.pose.orientation.x = q[0]
new_state.pose.orientation.y = q[1]
new_state.pose.orientation.z = q[2]
new_state.pose.orientation.w = q[3]
new_state.twist = curr_state.twist
setModelState(new_state)
def _cb_modeldata(self, msg):
self.data_to_publish = {}
for pos, item in enumerate(msg.name):
twist = msg.twist[pos]
pose = msg.pose[pos]
self.model_twists[item] = twist
self.model_poses[item] = pose
ms = ModelState()
ms.pose = pose
ms.twist = twist
ms.model_name = item
self.model_states[item] = ms
op_str = item + ", pose"
val_str = str(ms.pose)
self.data_to_publish[op_str] = str(val_str)
if item != self.name:
continue
self.pose = msg.pose[pos]
def _reset(self):
rospy.wait_for_service('gazebo/set_model_state')
self.num_target = np.random.randint(3)
self.num_hint = len(self.hint_pos[self.num_target])
self.setTarget()
state = ModelState()
state.model_name = self.name_model
state.reference_frame = "world"
state.pose = self.turtlebot_state.pose
state.twist = self.turtlebot_state.twist
self.set_model(state)
# Unpause simulation to make observation
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except rospy.ServiceException, e:
print("/gazebo/unpause_physics service call failed")
def setmodel():
pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=10)
rospy.init_node('set_model_state', anonymous=True)
rate = rospy.Rate(100) # 10hz
while not rospy.is_shutdown():
modelstate = get_state_call("quadrotor_imu")
msg = ModelState()
msg.pose = modelstate.pose
msg.twist = modelstate.twist
msg.pose.position.z = 3.0
msg.twist.linear.z = 0
msg.model_name = "quadrotor_imu"
pub.publish(msg)
rate.sleep()
def teleportRandom(self):
'''
Teleport agent return new x and y point
return agent posX, posY in list
'''
model_state_msg = ModelState()
model_state_msg.model_name = self.agent_model_name
# maze 1
xy_list = [[-2, 2], [-2, 1], [2, -2], [2, 2], [-1, 2], [-1, 1],
[1, -1], [1, 2]]
# Get random position for agent
pose = Pose()
pose.position.x, pose.position.y = random.choice(xy_list)
model_state_msg.pose = pose
model_state_msg.twist = Twist()
model_state_msg.reference_frame = "world"
# Start teleporting in Gazebo
isTeleportSuccess = False
for i in range(5):
if not isTeleportSuccess:
try:
rospy.wait_for_service('/gazebo/set_model_state')
telep_model_prox = rospy.ServiceProxy(
'/gazebo/set_model_state', SetModelState)
telep_model_prox(model_state_msg)
isTeleportSuccess = True
break
except Exception as e:
rospy.logfatal("Error when teleporting agent " + str(e))
else:
rospy.logwarn("Trying to teleporting agent..." + str(i))
time.sleep(2)
if not isTeleportSuccess:
rospy.logfatal("Error when teleporting agent")
return "Err", "Err"
return pose.position.x, pose.position.y
def moving_tile_x_long_path1(self):
obstacle = ModelState()
model = rospy.wait_for_message('gazebo/model_states', ModelStates)
model_original_pose_x = model.pose[0].position.x
for i in range(len(model.name)):
if model.name[i] == self.model_name and self.flag==1 and self.flag1 == 0 and self.flag2==0 and self.flag3 == 1 and round(model.pose[i].position.x) < round(self.x_model_pose)+3 :
obstacle.model_name = self.model_name
obstacle.pose = model.pose[i]
obstacle.twist = Twist()
obstacle.twist.linear.x = speed
obstacle.twist.angular.z = 0
self.pub_model.publish(obstacle)
elif model.name[i] == self.model_name and round(model.pose[i].position.x) >= 4.0:
print("1")
self.flag4 = 1
self.flag3 = 0
def movingTo(self, goal_x, goal_y, goal_z):
#pub_model = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=1)
obstacle = ModelState()
model = rospy.wait_for_message('gazebo/model_states', ModelStates)
#print(model.name)
#print(model.twist[2])
# print(type(obstacle))
#time.sleep(5)
for i in range(len(model.name)):
if model.name[
i] == 'bot': # the model name is defined in .xacro file
obstacle.model_name = 'bot'
obstacle.pose = model.pose[i]
obstacle.pose.position.x = float(goal_x)
obstacle.pose.position.y = float(goal_y)
obstacle.twist = Twist()
obstacle.twist.angular.z = float(goal_z)
self.pub_model.publish(obstacle)
def spawnMBase(x=DEF_X, y = DEF_Y):
mState = ModelState()
mState.model_name = BOT
pos = Point()
pos.x = x
pos.y = y
pos.z = 0.0
ori = Quaternion()
ori.x = 0.0
ori.y = 0.0
ori.z = 0.0
ori.w = 1.0
mState.pose.position = pos
mState.pose.orientation = ori
mState.twist = twist_no_vel()
mState.reference_frame = 'world'
stop()
status = serviceThis(SET_MODEL_STATE, SetModelState, mState)
mbState = mState
def reset_simulation(self):
# Empty the self.model_polled_z list
self.model_polled_z = []
# Send model to x=0,y=0
model_name = 'darwin'
pose = Pose()
pose.position.z = 0.31
twist = Twist()
reference_frame = 'world'
md_state = ModelState()
md_state.model_name = model_name
md_state.pose = pose
md_state.twist = twist
md_state.reference_frame = reference_frame
# Service call to reset model
try:
response = self.set_model_state(md_state)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def on_set_to_position_clicked_(self):
success = True
set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
model_state = ModelState()
model_state.model_name = str(self._widget.comboBoxObjectList.currentText()).split('::')[0]
model_state.pose = Pose()
model_state.twist = Twist()
model_state.reference_frame = "world"
model_state.pose.position.x = float(str(self._widget.lineEdit_4.text()))
model_state.pose.position.y = float(str(self._widget.lineEdit_5.text()))
model_state.pose.position.z = float(str(self._widget.lineEdit_6.text()))
try:
resp1 = set_model_state(model_state)
except rospy.ServiceException:
success = False
if success:
if not resp1.success:
success = False
if not success:
QMessageBox.warning(self._widget, "Warning", "Could not set model state.")
status = (status + 1) % 15
elif key == 'k':
x = 0
th = 0
control_speed = 0
control_turn = 0
elif key == ' ':
# move human up stairs
old_state = get_state_client.call("human_movable", "world")
new_state = ModelState()
new_state.model_name = "human_movable"
new_state.reference_frame = "world"
new_state.pose = old_state.pose
new_state.twist = old_state.twist
new_state.pose.position.x -= 0.5
new_state.pose.position.z += 0.15
set_state_client.call(new_state)
else:
count += 1
if count > 4:
x = 0
th = 0
if key == '\x03':
break
target_speed = speed * x
target_turn = turn * th
