def getDataSendContainer(data_topic): if data_topic == 'cmd_vel': return Twist() elif data_topic == 'husky_sim_speed': model_state = ModelState() model_state.model_name = 'husky' return model_state
def animate():
pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=1)
rospy.init_node('animate_ball')
rate = rospy.Rate(20) # 15 Hz
x = 0.9
y = 0.0
z = 1.2
ballState = ModelState()
ballState.model_name = 'ball'
ballState.pose.orientation.x = 0
ballState.pose.orientation.y = 0
ballState.pose.orientation.z = 0
ballState.pose.orientation.w = 1
ballState.reference_frame = 'world'
direction = 'left'
step = 0.08
radius = 0.1
angle = 0
while not rospy.is_shutdown():
ballState.pose.position.x = x
ballState.pose.position.y = y + radius * cos(angle)
ballState.pose.position.z = z + radius * sin(angle)
angle = angle + step % pi
pub.publish(ballState)
rate.sleep()
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 cbEncoder(self, encoder_msg): delta_R = (encoder_msg.x - self.previousR) * self.d_PerCount delta_L= (encoder_msg.y - self.previousL) * self.d_PerCount delta_S = (delta_R + delta_L)/2 # print "encoder_msg.x = ",encoder_msg.x," encoder_msg.y = ",encoder_msg.y #print "previousR = "******" previousL = ",self.previousL self.th = ((delta_L - delta_R)/self.Length + self.th) self.x = delta_S * math.cos(self.th) + self.x self.y = delta_S * math.sin(self.th) + self.y model_state_msg = ModelState() model_state_msg.model_name = 'duckiebot_with_gripper' model_state_msg.pose.position.x = self.x model_state_msg.pose.position.y = self.y model_state_msg.pose.position.z = 0 new_quaternion = tf.transformations.quaternion_from_euler(0, 0, self.th) model_state_msg.pose.orientation.x = new_quaternion[0] model_state_msg.pose.orientation.y = new_quaternion[1] model_state_msg.pose.orientation.z = new_quaternion[2] model_state_msg.pose.orientation.w = new_quaternion[3] self.pub_gazebo.publish(model_state_msg) self.previousR = encoder_msg.x self.previousL = encoder_msg.y
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 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 racecar_reset(self):
rospy.wait_for_service('gazebo/set_model_state')
modelState = ModelState()
modelState.pose.position.z = 0
modelState.pose.orientation.x = 0
modelState.pose.orientation.y = 0
modelState.pose.orientation.z = 0
modelState.pose.orientation.w = 0 # Use this to randomize the orientation of the car
modelState.twist.linear.x = 0
modelState.twist.linear.y = 0
modelState.twist.linear.z = 0
modelState.twist.angular.x = 0
modelState.twist.angular.y = 0
modelState.twist.angular.z = 0
modelState.model_name = 'racecar'
if self.world_name.startswith(MEDIUM_TRACK_WORLD):
modelState.pose.position.x = -1.40
modelState.pose.position.y = 2.13
elif self.world_name.startswith(EASY_TRACK_WORLD):
modelState.pose.position.x = -1.44
modelState.pose.position.y = -0.06
elif self.world_name.startswith(HARD_TRACK_WORLD):
modelState.pose.position.x = 1.75
modelState.pose.position.y = 0.6
def toQuaternion(pitch, roll, yaw):
cy = np.cos(yaw * 0.5)
sy = np.sin(yaw * 0.5)
cr = np.cos(roll * 0.5)
sr = np.sin(roll * 0.5)
cp = np.cos(pitch * 0.5)
sp = np.sin(pitch * 0.5)
w = cy * cr * cp + sy * sr * sp
x = cy * sr * cp - sy * cr * sp
y = cy * cr * sp + sy * sr * cp
z = sy * cr * cp - cy * sr * sp
return [x, y, z, w]
#clockwise
quaternion = toQuaternion(roll=0.0, pitch=0.0, yaw=np.pi)
#anti-clockwise
quaternion = toQuaternion(roll=0.0, pitch=0.0, yaw=0.0)
modelState.pose.orientation.x = quaternion[0]
modelState.pose.orientation.y = quaternion[1]
modelState.pose.orientation.z = quaternion[2]
modelState.pose.orientation.w = quaternion[3]
else:
raise ValueError("Unknown simulation world: {}".format(self.world_name))
self.racecar_service(modelState)
time.sleep(SLEEP_AFTER_RESET_TIME_IN_SECOND)
self.progress_at_beginning_of_race = self.progress
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 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 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 turtlebot3_reset(self):
rospy.wait_for_service('gazebo/set_model_state')
self.x = 0
self.y = 0
# Put the turtlebot waffle at (0, 0)
modelState = ModelState()
modelState.pose.position.z = 0
modelState.pose.orientation.x = 0
modelState.pose.orientation.y = 0
modelState.pose.orientation.z = 0
modelState.pose.orientation.w = 0
modelState.twist.linear.x = 0
modelState.twist.linear.y = 0
modelState.twist.linear.z = 0
modelState.twist.angular.x = 0
modelState.twist.angular.y = 0
modelState.twist.angular.z = 0
modelState.model_name = 'turtlebot3'
modelState.pose.position.x = self.x
modelState.pose.position.y = self.y
self.gazebo_model_state_service(modelState)
self.burger_x = 3.5
self.burger_y = random.uniform(-1, 1)
# Put the turtlebot burger at (2, 0)
modelState = ModelState()
modelState.pose.position.z = 0
modelState.pose.orientation.x = 0
modelState.pose.orientation.y = 0
modelState.pose.orientation.z = 0
modelState.pose.orientation.w = random.uniform(0, 3)
modelState.twist.linear.x = 0
modelState.twist.linear.y = 0
modelState.twist.linear.z = 0
modelState.twist.angular.x = 0
modelState.twist.angular.y = 0
modelState.twist.angular.z = 0
modelState.model_name = 'turtlebot3_burger'
modelState.pose.position.x = self.burger_x
modelState.pose.position.y = self.burger_y
self.gazebo_model_state_service(modelState)
self.last_distance_of_turtlebot = sys.maxsize
time.sleep(SLEEP_AFTER_RESET_TIME_IN_SECOND)
def handle_position_change(self,pose):
model_state = ModelState()
model_state.model_name = "teresa_robot"
model_state.pose = pose.pose
model_state.reference_frame="map"
for i in range(25):
self.gazebo_pub.publish(model_state)
rospy.sleep(0.03)
for i in range(25):
amcl_pose = PoseWithCovarianceStamped()
amcl_pose.pose.pose = pose.pose
amcl_pose.header.frame_id = "map"
amcl_pose.header.stamp = rospy.get_rostime()
self.amcl_pub.publish(amcl_pose)
rospy.sleep(0.03)
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 spawnMBase(x, y):
mState = ModelState()
mState.model_name = 'mobile_base'
mState.pose.position.x = x
mState.pose.position.y = y
mState.pose.position.z = 0
mState.pose.orientation.x = 0
mState.pose.orientation.y = 0
mState.pose.orientation.z = 0
mState.pose.orientation.w = 1
rp.wait_for_service('/gazebo/set_model_state')
try:
sms = rp.ServiceProxy('/gazebo/set_model_state', SetModelState)
print 'Model State: ', str(mState)
sms(mState)
except rp.ServiceException as e:
print 'Service Exception:', e
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.")
if msg.buttons[0]:
if state < 5:
state += 1
time.sleep(7)
elif state == 5:
state = 0
time.sleep(7)
rospy.init_node('sin_path', anonymous=True)
swarm_pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=1000)
container_pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=10)
rospy.Subscriber('/joy', Joy, joy_callback)
container_msg = ModelState()
container_msg.model_name = 'box'
container_msg.pose.position.x = 0
container_msg.pose.position.y = 0
container_msg.pose.position.z = 0.5
swarm_msg = [ModelState() for _ in xrange(25)]
for i in xrange(25):
swarm_msg[i].model_name = 'quadrotor' + str(i)
dt = 0.1
x = 0
y = 0
z = 0
x_values = np.arange(-5, 5, 0.0009, dtype=None)
def racecar_reset(self):
rospy.wait_for_service('gazebo/set_model_state')
modelState = ModelState()
modelState.pose.position.z = 0
modelState.pose.orientation.x = 0
modelState.pose.orientation.y = 0
modelState.pose.orientation.z = 0
# Use this to randomize the orientation of the car
modelState.pose.orientation.w = 0
modelState.twist.linear.x = 0
modelState.twist.linear.y = 0
modelState.twist.linear.z = 0
modelState.twist.angular.x = 0
modelState.twist.angular.y = 0
modelState.twist.angular.z = 0
modelState.model_name = 'racecar'
if self.world_name.startswith(MEDIUM_TRACK_WORLD):
modelState.pose.position.x = -1.40
modelState.pose.position.y = 2.13
elif self.world_name.startswith(EASY_TRACK_WORLD):
modelState.pose.position.x = -1.44
modelState.pose.position.y = -0.06
elif self.world_name.startswith(HARD_TRACK_WORLD):
wayPoints = [2, 23]
wayPoint = random.choice(wayPoints)
modelState.pose.position.x = self.waypoints[wayPoint][0]
modelState.pose.position.y = self.waypoints[wayPoint][1]
if wayPoint < 5:
yaw = 0.0
else:
yaw = math.pi
def toQuaternion(pitch, roll, yaw):
cy = np.cos(yaw * 0.5)
sy = np.sin(yaw * 0.5)
cr = np.cos(roll * 0.5)
sr = np.sin(roll * 0.5)
cp = np.cos(pitch * 0.5)
sp = np.sin(pitch * 0.5)
w = cy * cr * cp + sy * sr * sp
x = cy * sr * cp - sy * cr * sp
y = cy * cr * sp + sy * sr * cp
z = sy * cr * cp - cy * sr * sp
return [x, y, z, w]
# clockwise
quaternion = toQuaternion(roll=0.0, pitch=0.0, yaw=np.pi)
# anti-clockwise
quaternion = toQuaternion(roll=0.0, pitch=0.0, yaw=yaw)
modelState.pose.orientation.x = quaternion[0]
modelState.pose.orientation.y = quaternion[1]
modelState.pose.orientation.z = quaternion[2]
modelState.pose.orientation.w = quaternion[3]
else:
raise ValueError("Unknown simulation world: {}".format(
self.world_name))
self.racecar_service(modelState)
time.sleep(SLEEP_AFTER_RESET_TIME_IN_SECOND)
self.progress_at_beginning_of_race = self.progress
def reset(self):
# Resets the state of the environment and returns an initial observation.
rospy.wait_for_service('/gazebo/reset_simulation')
try:
self.reset_proxy()
pose = Pose()
pose.position.x = np.random.uniform(low=START_STATE[0] - 0.5,
high=START_STATE[0] + 0.5)
pose.position.y = np.random.uniform(low=START_STATE[1] - 0.5,
high=START_STATE[1] + 0.5)
pose.position.z = self.get_model_states(
model_name="mobile_base").pose.position.z
theta = np.random.uniform(low=START_STATE[2],
high=START_STATE[2] + np.pi)
ox, oy, oz, ow = quaternion_from_euler(0.0, 0.0, theta)
pose.orientation.x = ox
pose.orientation.y = oy
pose.orientation.z = oz
pose.orientation.w = ow
reset_state = ModelState()
reset_state.model_name = "mobile_base"
reset_state.pose = pose
self.set_model_states(reset_state)
except rospy.ServiceException as e:
print(
"# Resets the state of the environment and returns an initial observation."
)
# Unpause simulation to make observation
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except rospy.ServiceException as e:
print("/gazebo/unpause_physics service call failed")
# print("successfully unpaused!!")
#laser_data = rospy.wait_for_message('/scan', LaserScan, timeout=5)
# read laser data
laser_data = None
dynamic_data = None
while laser_data is None and dynamic_data is None:
try:
laser_data = rospy.wait_for_message('/scan',
LaserScan,
timeout=5)
# dynamic_data = rospy.wait_for_message('/gazebo/model_states', ModelStates, timeout=5)
rospy.wait_for_service("/gazebo/get_model_state")
try:
dynamic_data = self.get_model_states(
model_name="mobile_base")
# dynamic_data = self.get_model_states(model_name="dubins_car")
except rospy.ServiceException as e:
print("/gazebo/unpause_physics service call failed")
except:
pass
# print("laser data", laser_data)
# print("dynamics data", dynamic_data)
rospy.wait_for_service('/gazebo/pause_physics')
try:
self.pause()
except rospy.ServiceException as e:
print("/gazebo/pause_physics service call failed")
obsrv = self.get_obsrv(laser_data, dynamic_data)
self.pre_obsrv = obsrv
# print("successfully reset!!")
return np.asarray(obsrv)
def respawn(self, spawning):
self.spawning = spawning
if self.spawn == True or self.spawning == True:
self.counter = 20000
self.counter += 1
# print self.counter
else:
self.counter = 0
self.spawn_publisher = rospy.Publisher("/gazebo/set_model_state",
ModelState,
queue_size=1)
if self.vehicle_name == "fusion":
xx = 55.2
yy = -100
zz = 0.3
yaw = 0
else:
xx = -87.7943398547
yy = 394.797117511
zz = 0.3
yaw = -1
states = ModelState()
# print self.spawning, "is the actual value"
if self.counter == 0:
posex = self.odom_data[0][0]
posey = self.odom_data[0][1]
posez = self.odom_data[0][2]
poseox = self.odom_data[1][0]
poseoy = self.odom_data[1][1]
poseoz = self.odom_data[1][2]
poseow = self.odom_data[1][3]
twistx = self.linear_velocity[0]
twisty = self.linear_velocity[1]
twistz = self.linear_velocity[2]
twistax = self.angular_velocity[0]
twistay = self.angular_velocity[1]
twistaz = self.angular_velocity[2]
return False
while self.counter > 20000:
if 20000 <= self.counter < 20002:
posex = self.odom_data[0][0]
posey = self.odom_data[0][1]
posez = self.odom_data[0][2]
poseox = self.odom_data[1][0]
poseoy = self.odom_data[1][1]
poseoz = self.odom_data[1][2]
poseow = self.odom_data[1][3]
twistx = self.twist_data[0][0]
twisty = self.twist_data[0][1]
twistz = self.twist_data[0][2]
twistax = self.twist_data[1][0]
twistay = self.twist_data[1][1]
twistaz = self.twist_data[1][2]
self.counter += 1
time.sleep(0.2)
elif 20002 <= self.counter < 20004:
posex = xx
posey = yy
posez = zz
poseox = 0
poseoy = 0
poseoz = yaw
poseow = 0
twistx = 0
twisty = 0
twistz = 0
twistax = 0
twistay = 0
twistaz = 0
self.counter += 1
time.sleep(0.2)
if self.counter >= 20004:
# print self.counter
self.counter = 0
# print self.counter
states.model_name = "%s" % (self.vehicle_name)
states.twist.linear.x = twistx
states.twist.linear.y = twisty
states.twist.linear.z = twistz
states.twist.angular.x = twistax
states.twist.angular.y = twistay
states.twist.angular.z = twistaz
states.pose.position.x = posex
states.pose.position.y = posey
states.pose.position.z = posez
states.pose.orientation.x = poseox
states.pose.orientation.y = poseoy
states.pose.orientation.z = poseoz
states.pose.orientation.w = poseow
self.spawn_publisher.publish(states)
return False
def __init__(self, robot_name, max_steps):
#------------Params--------------------
self.depth_image_size = [160, 128]
self.rgb_image_size = [304, 228]
self.bridge = CvBridge()
self.robot_name = robot_name
self.self_speed = [0.0, 0.0]
self.start_time = time.time()
self.max_steps = max_steps
self.sim_time = Clock().clock
self.control_freq = 5.
self.pose = None
default_positions = {
'robot1': [11., 0., 0.],
'robot2': [11., 1., 0.],
'robot3': [11., 2., 0.],
'robot4': [11., 3., 0.],
'robot5': [11., 4., 0.]
}
#-----------Default Robot State-----------------------
self.default_state = ModelState()
self.default_state.model_name = robot_name
self.default_state.pose.position.x = default_positions[robot_name][0]
self.default_state.pose.position.y = default_positions[robot_name][1]
self.default_state.pose.position.z = default_positions[robot_name][2]
self.default_state.pose.orientation.x = 0.0
self.default_state.pose.orientation.y = 0.0
self.default_state.pose.orientation.z = 0.0
self.default_state.pose.orientation.w = 1.0
self.default_state.twist.linear.x = 0.
self.default_state.twist.linear.y = 0.
self.default_state.twist.linear.z = 0.
self.default_state.twist.angular.x = 0.
self.default_state.twist.angular.y = 0.
self.default_state.twist.angular.z = 0.
self.default_state.reference_frame = 'world'
#-----------Publisher and Subscriber-------------
self.cmd_vel = rospy.Publisher(robot_name +
'/mobile_base/commands/velocity',
Twist,
queue_size=10)
self.set_state = rospy.Publisher('gazebo/set_model_state',
ModelState,
queue_size=100)
self.resized_depth_img = rospy.Publisher(robot_name +
'/camera/depth/image_resized',
Image,
queue_size=10)
self.resized_rgb_img = rospy.Publisher(robot_name +
'/camera/rgb/image_resized',
Image,
queue_size=10)
self.object_state_sub = rospy.Subscriber('gazebo/model_states',
ModelStates,
self.ModelStateCallBack)
self.odom_sub = rospy.Subscriber(robot_name + '/odom', Odometry,
self.OdometryCallBack)
self.sim_clock = rospy.Subscriber('clock', Clock,
self.SimClockCallBack)
self.depth_image_sub = rospy.Subscriber(
robot_name + '/camera/depth/image_raw', Image,
self.DepthImageCallBack)
self.rgb_image_sub = rospy.Subscriber(
robot_name + '/camera/rgb/image_raw', Image, self.RGBImageCallBack)
self.laser_sub = rospy.Subscriber('mybot/laser/scan', LaserScan,
self.LaserScanCallBack)
is_first_while = True
pre_test_number = -1
rospy.loginfo(log_file_path)
while not rospy.is_shutdown():
if test_number != pre_test_number:
end_time = rospy.Time.now()
if not is_first_while:
elapsed_time = (end_time - start_time).to_sec()
rospy.loginfo("{}".format(elapsed_time))
f = open(log_file_path, 'a')
f.write("{}, {}\n".format(path_list[test_number], elapsed_time))
f.close()
is_first_while = False
rospy.wait_for_service('gazebo/set_model_state')
init_pose = Pose(position=Point(-1,0,1.5), orientation=Quaternion(0, 0, 0.7071068, 0.7071068))
model_init_state = ModelState(model_name='ackermann_vehicle', pose=init_pose,
twist=Twist(), reference_frame="world")
spawn_model_prox = rospy.ServiceProxy('gazebo/set_model_state', SetModelState)
spawn_model_prox(model_init_state)
path_reader(path_list[test_number])
rospy.loginfo("{} {}".format(test_number, path_list[test_number]))
pre_test_number = test_number
is_first_while = False
wp_index = 1
vehicle_status = "Generated"
start_time = end_time
try:
rospy.spin()
except rospy.ROSInterruptException:
pass
def pub_odometry(self):
""" Publish odometry message """
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = self.frame_id
odom.child_frame_id = self.world_frame_id
odom.pose.pose.position.x = self.p[0]
odom.pose.pose.position.y = self.p[1]
odom.pose.pose.position.z = self.p[2]
orientation = tf.transformations.quaternion_from_euler(
self.p[3], self.p[4], self.p[5], 'sxyz')
odom.pose.pose.orientation.x = orientation[0]
odom.pose.pose.orientation.y = orientation[1]
odom.pose.pose.orientation.z = orientation[2]
odom.pose.pose.orientation.w = orientation[3]
odom.twist.twist.linear.x = self.v[0]
odom.twist.twist.linear.y = self.v[1]
odom.twist.twist.linear.z = self.v[2]
odom.twist.twist.angular.x = self.v[3]
odom.twist.twist.angular.y = self.v[4]
odom.twist.twist.angular.z = self.v[5]
self.pub_odom.publish(odom)
# Broadcast transform
br = tf.TransformBroadcaster()
br.sendTransform((self.p[0], self.p[1], self.p[2]), orientation,
odom.header.stamp, odom.header.frame_id,
self.world_frame_id)
################################################################
########### Publish position for gazebo ################
################################################################
gazebo_odom = ModelState()
###### TODO: WARNING! Gazebo uses Z up configuraton!!! ###
# I've created a custom rotation that rotates the position 180 degress
# in roll, but the orientation transformation is only yaw = -yaw.
# CHECK WHAT HAPPENS WHITH ROLL AND PITCH!
"""rot = tf.transformations.euler_matrix(math.pi, 0.0, 0.0)
position = np.matrix([odom.pose.pose.position.x,
odom.pose.pose.position.y,
odom.pose.pose.position.z,
1.0]).reshape(4, 1)
new_position = rot * position
eulr = tf.transformations.euler_from_quaternion([odom.pose.pose.orientation.x,
odom.pose.pose.orientation.y,
odom.pose.pose.orientation.z,
odom.pose.pose.orientation.w])
new_quat = tf.transformations.quaternion_from_euler(eulr[0], eulr[1], -eulr[2])
gazebo_odom.model_name = 'girona500'
gazebo_odom.pose.position.x = float(new_position[0])
gazebo_odom.pose.position.y = float(new_position[1])
gazebo_odom.pose.position.z = float(new_position[2]) + self.sea_bottom_depth
gazebo_odom.pose.orientation = odom.pose.pose.orientation
gazebo_odom.pose.orientation.x = new_quat[0]
gazebo_odom.pose.orientation.y = new_quat[1]
gazebo_odom.pose.orientation.z = new_quat[2]
gazebo_odom.pose.orientation.w = new_quat[3]
gazebo_odom.reference_frame = 'world'
self.pub_odom_gazebo.publish(gazebo_odom)
# Brodcast world to girona50000_gazebo_link
br = tf.TransformBroadcaster()
br.sendTransform((gazebo_odom.pose.position.x,
gazebo_odom.pose.position.y,
gazebo_odom.pose.position.z),
new_quat,
odom.header.stamp,
"girona500_gazebo_link",
self.world_frame_id) """
##################################################################
# ALTERNATIVE WITH NO ROTATIONS #####
gazebo_odom.model_name = 'girona500'
gazebo_odom.pose = odom.pose.pose
gazebo_odom.reference_frame = 'world'
self.pub_odom_gazebo.publish(gazebo_odom)
def __init__(self):
# initiliaze
rospy.init_node('GazeboWorld', anonymous=False)
#-----------Default Robot State-----------------------
self.set_self_state = ModelState()
self.set_self_state.model_name = 'mobile_base'
self.set_self_state.pose.position.x = 0.
self.set_self_state.pose.position.y = 2.5
self.set_self_state.pose.position.z = 0.
self.set_self_state.pose.orientation.x = 0.0
self.set_self_state.pose.orientation.y = 0.0
self.set_self_state.pose.orientation.z = 0.0
self.set_self_state.pose.orientation.w = 1.0
self.set_self_state.twist.linear.x = 0.
self.set_self_state.twist.linear.y = 0.
self.set_self_state.twist.linear.z = 0.
self.set_self_state.twist.angular.x = 0.
self.set_self_state.twist.angular.y = 0.
self.set_self_state.twist.angular.z = 0.
self.set_self_state.reference_frame = 'world'
#------------Params--------------------
self.depth_image_size = [160, 128]
self.rgb_image_size = [304, 228]
self.bridge = CvBridge()
self.object_state = [0, 0, 0, 0]
self.object_name = []
# 0. | left 90/s | left 45/s | right 45/s | right 90/s | acc 1/s | slow down -1/s
self.action_table = [
[0.5, np.pi / 12], [0.5, 0], [0.5, -np.pi / 12],
[0.2, np.pi / 6], [0.2, -np.pi / 6], [0.2, np.pi / 12],
[0.2,
-np.pi / 12], [0.2,
0], [0, np.pi / 6], [0, -np.pi / 6]
] #[0.4, 0.2, np.pi/6, np.pi/12, 0., -np.pi/12, -np.pi/6]
self.self_speed = [.4, 0.0]
self.default_states = None
self.start_time = time.time()
self.max_steps = 10000
self.beam_num = 512
self.depth_image = None
self.bump = False
#-----------Publisher and Subscriber-------------
self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi',
Twist,
queue_size=10)
self.set_state = rospy.Publisher('gazebo/set_model_state',
ModelState,
queue_size=10)
self.resized_depth_img = rospy.Publisher('camera/depth/image_resized',
Image,
queue_size=10)
self.resized_rgb_img = rospy.Publisher('camera/rgb/image_resized',
Image,
queue_size=10)
self.object_state_sub = rospy.Subscriber('gazebo/model_states',
ModelStates,
self.ModelStateCallBack)
self.depth_image_sub = rospy.Subscriber('camera/rgb/image_raw', Image,
self.RGBImageCallBack)
self.rgb_image_sub = rospy.Subscriber('camera/depth/image_raw', Image,
self.DepthImageCallBack)
self.laser_sub = rospy.Subscriber('scan', LaserScan,
self.LaserScanCallBack)
self.odom_sub = rospy.Subscriber('odom', Odometry,
self.OdometryCallBack)
self.bumper_sub = rospy.Subscriber('mobile_base/events/bumper',
BumperEvent, self.BumperCallBack)
rospy.sleep(2.)
# What function to call when you ctrl + c
rospy.on_shutdown(self.shutdown)
def main():
global regions_, position_, desired_position_, state_, yaw_, yaw_error_allowed_
global srv_client_go_to_point_, srv_client_wall_follower_
rospy.init_node('bug0')
sub_laser = rospy.Subscriber('/diff_bot/laser/scan', LaserScan, clbk_laser)
sub_odom = rospy.Subscriber('/odom', Odometry, clbk_odom)
rospy.wait_for_service('/go_to_point_switch')
rospy.wait_for_service('/wall_follower_switch')
rospy.wait_for_service('/gazebo/set_model_state')
srv_client_go_to_point_ = rospy.ServiceProxy('/go_to_point_switch',
SetBool)
srv_client_wall_follower_ = rospy.ServiceProxy('/wall_follower_switch',
SetBool)
srv_client_set_model_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
# set robot position
model_state = ModelState()
model_state.model_name = 'diff_bot'
model_state.pose.position.x = initial_position_.x
model_state.pose.position.y = initial_position_.y
resp = srv_client_set_model_state(model_state)
# initialize going to the point
change_state(0)
rate = rospy.Rate(20)
while not rospy.is_shutdown():
if regions_ == None:
continue
if state_ == 0:
if regions_['front'] > 0.15 and regions_['front'] < 1:
change_state(1)
elif state_ == 1:
desired_yaw = math.atan2(desired_position_.y - position_.y,
desired_position_.x - position_.x)
err_yaw = normalize_angle(desired_yaw - yaw_)
# less than 30 degrees
if math.fabs(err_yaw) < (math.pi / 6) and \
regions_['front'] > 1.5 and regions_['fright'] > 1 and regions_['fleft'] > 1:
print 'less than 30'
change_state(0)
# between 30 and 90
if err_yaw > 0 and \
math.fabs(err_yaw) > (math.pi / 6) and \
math.fabs(err_yaw) < (math.pi / 2) and \
regions_['left'] > 1.5 and regions_['fleft'] > 1:
print 'between 30 and 90 - to the left'
change_state(0)
if err_yaw < 0 and \
math.fabs(err_yaw) > (math.pi / 6) and \
math.fabs(err_yaw) < (math.pi / 2) and \
regions_['right'] > 1.5 and regions_['fright'] > 1:
print 'between 30 and 90 - to the right'
change_state(0)
rate.sleep()
rospy.logerr('Error on calling service: %s',str(e))
pose = Pose()
pose.position.x = rospy.get_param('~position_x')
pose.position.y = rospy.get_param('~position_y')
pose.position.z = rospy.get_param('~position_z')
pose.orientation.x = rospy.get_param('~orientation_x')
pose.orientation.y = rospy.get_param('~orientation_y')
pose.orientation.z = rospy.get_param('~orientation_z')
pose.orientation.w = rospy.get_param('~orientation_w')
state = ModelState()
state.model_name = "robot"
state.pose = pose
rospy.loginfo("Moving robot")
try:
ret = g_set_state(state)
print ret.status_message
except Exception, e:
rospy.logerr('Error on calling service: %s',str(e))
#-*-coding: utf8-*-
"""
Remplace le systeme VICON pour la simulation en recuperant la position des objets
dans gazebo et en les publiant.
Met également à jour les positions des objectifs des drones dans gazebo (invisible
en realite).
"""
import rospy
from tf.transformations import euler_from_quaternion, quaternion_from_euler
from gazebo_msgs.msg import ModelState, ModelStates
from geometry_msgs.msg import PoseStamped, TransformStamped
pos_leader = [0, 0, 0, 0]
pos_follower = [0, 0, 0, 0]
ms_leader = ModelState()
ms_follower = ModelState()
def recupere_pos(msg):
global pos_leader, pos_follower
#leader
i = msg.name.index("uav1")
pos_leader[0] = msg.pose[i].position.x
pos_leader[1] = msg.pose[i].position.y
pos_leader[2] = msg.pose[i].position.z
q = (msg.pose[i].orientation.x, msg.pose[i].orientation.y,
msg.pose[i].orientation.z, msg.pose[i].orientation.w)
euler = euler_from_quaternion(q)
pos_leader[3] = euler[2]
if abs(X) > math.pi / 2 or abs(Y) > math.pi / 2:
return True
else:
return False
def reset(self):
rospy.wait_for_service('/gazebo/reset_world')
try:
reset_world_handle = rospy.ServiceProxy('/gazebo/reset_world',
Empty)
reset_world_handle()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
init_model_state = ModelState()
if self.random_seed != None:
if self.random_seed != 0:
random.seed(self.random_seed)
init_model_state.model_name = 'iris'
init_model_state.pose.position.x = random.uniform(-3, 3)
init_model_state.pose.position.y = random.uniform(-3, 3)
init_model_state.pose.position.z = random.uniform(2, 8)
w, x, y, z = self.randQ()
init_model_state.pose.orientation.x = x
init_model_state.pose.orientation.y = y
init_model_state.pose.orientation.z = z
init_model_state.pose.orientation.w = w
init_model_state.twist.linear.x = random.uniform(-1, 1)
init_model_state.twist.linear.y = random.uniform(-1, 1)
init_model_state.twist.linear.z = random.uniform(-1, 1)
def reset(self, episode_num, collision):
print "Resetting Baxter..."
# self.delete_model("hugging_target")
# rospy.sleep(0.1)
model_msg = ModelState()
model_msg.model_name = "humanoid"
model_msg.reference_frame = "world"
model_msg.pose.position.x = 2.0
model_msg.pose.position.y = 0.0
model_msg.pose.position.z = 0
# self.set_model_config('hugging_target', 'humanoids',
# ['LeftLeg_joint', 'RightLeg_joint', 'RightUpperArm_joint', 'Head_joint'],
# [0.0, 0.0, 0.0, 0.0])
resp_set = self.set_model_state(model_msg)
# reset arm position
if not self.use_moveit:
# right arm
start_point_right = [-0.3, 1.0, 0.0, 0.5, 0.0, 0.027, 0.0]
t01 = threading.Thread(target=resetarm_job,
args=(self.right_limb_interface,
start_point_right))
t01.start()
# left arm
start_point_left = [0.3, 1.0, 0.0, 0.5, 0.0, 0.027, 0.0]
t02 = threading.Thread(target=resetarm_job,
args=(self.left_limb_interface,
start_point_left))
t02.start()
t02.join()
t01.join()
else:
# Moveit joint move
joint_goal = self.group.get_current_joint_values()
# s0, s1, e0, e1, w0, w1, w2
joint_goal[0] = 0.0
joint_goal[1] = -0.55
joint_goal[2] = 0.0
joint_goal[3] = 0.75
joint_goal[4] = 0.0
joint_goal[5] = 1.26
joint_goal[6] = 0.0
self.group.go(joint_goal, wait=True)
self.group.stop()
# ###################### Reset hugging target ##########################
if self.hug_target == 1:
# Randomly initialize target position
self.cylinder1[0] = random.uniform(0.3, 0.8)
self.cylinder1[1] = random.uniform(-0.2, 0.2)
cylinder_z = self.cylinder_height / 2.0
for i in range(len(self.target_line_start)):
self.target_line_start[i] = self.cylinder1 + asarray(
[0, 0, self.cylinder_height]) / 9.0 * i
self.target_line = self.target_line_start
print "load gazebo model"
resp = self.load_model(
"hugging_target", "cylinder.sdf",
Pose(position=Point(
x=self.cylinder1[0], y=self.cylinder1[1], z=cylinder_z)))
if self.hug_target == 2:
self.target_line_start = self.target_line_start - self.target_pos_start
self.target_pos_start[0] = 0.6 #random.uniform(0.4, 0.8)
self.target_pos_start[1] = 0.0 #random.uniform(-0.2, 0.2)
self.target_line_start = self.target_line_start + self.target_pos_start
self.target_line = self.target_line_start
print "load gazebo model"
# resp = self.load_model("hugging_target", "humanoid/humanoid.urdf",
# Pose(position=Point(x=self.target_pos_start[0], y=self.target_pos_start[1], z=0)), type="urdf")
# rospy.sleep(0.1)
model_msg = ModelState()
model_msg.model_name = "humanoid"
model_msg.reference_frame = "world"
model_msg.pose.position.x = self.target_pos_start[0]
model_msg.pose.position.y = self.target_pos_start[1]
model_msg.pose.position.z = -0.5
resp_set = self.set_model_state(model_msg)
rospy.sleep(0.5)
# Listen to collision information
# rospy.Subscriber(self.collision_topic, String, self.collision_getter)
if self.use_moveit:
# Delete object in the scene
count = 0
while 'target' in self.scene.getKnownCollisionObjects():
count += 1
if count > 10:
self.scene._collision = []
self.scene.removeCollisionObject('target', wait=True)
rospy.sleep(0.1)
print "deleting target...",
cylinder_name = "target"
cylinder_height = self.cylinder_height
# Add object to the planning scene for collision avoidance
while 'target' not in self.scene.getKnownCollisionObjects():
self.scene.addCylinder(
cylinder_name, cylinder_height, self.cylinder_radius,
self.cylinder1[0], self.cylinder1[1],
self.cylinder1[2] + cylinder_height / 2.0)
rospy.sleep(0.1)
print "adding target..."
print "cylinder_x: %f, cylinder_y: %f" % (self.cylinder1[0],
self.cylinder1[1]),
print "done"
def reset(self):
'''
Resettng the Experiment
1. Update the counter based on the flag from step
2. Assign next positions and reset the positions of robots and targets
'''
# ========================================================================= #
# 1. TARGET UPDATE #
# ========================================================================= #
self.is_collided = False
angle_target = self.target_index * 2 * pi / self.num_experiments
self.pos_x = self.radius * cos(angle_target)
self.pos_y = self.radius * sin(angle_target)
if self.target_index < self.num_experiments - 1:
self.target_index += 1
else:
self.target_index = 0
self.theta = angle_target
quat = quaternion_from_euler(0, 0, self.theta + pi)
# ========================================================================= #
# 2. RESET #
# ========================================================================= #
# Reset Turtlebot position
state_msg = ModelState()
state_msg.model_name = 'turtlebot3_waffle_pi'
state_msg.pose.position.x = -0.5
state_msg.pose.position.y = 0.0
state_msg.pose.position.z = 0.0
state_msg.pose.orientation.x = 0
state_msg.pose.orientation.y = 0
state_msg.pose.orientation.z = 0
state_msg.pose.orientation.w = 0
# state_msg.pose.position.x = self.pos_x
# state_msg.pose.position.y = self.pos_y
# state_msg.pose.position.z = 0.0
# state_msg.pose.orientation.x = quat[0]
# state_msg.pose.orientation.y = quat[1]
# state_msg.pose.orientation.z = quat[2]
# state_msg.pose.orientation.w = quat[3]
# Reset Target Position
state_target_msg = ModelState()
state_target_msg.model_name = 'unit_sphere' #'unit_sphere_0_0' #'unit_box_1' #'cube_20k_0'
state_target_msg.pose.position.x = self.target_x
state_target_msg.pose.position.y = self.target_y
state_target_msg.pose.position.z = 0.0
state_target_msg.pose.orientation.x = 0
state_target_msg.pose.orientation.y = 0
state_target_msg.pose.orientation.z = -0.2
state_target_msg.pose.orientation.w = 0
rospy.wait_for_service('gazebo/reset_simulation')
rospy.wait_for_service('/gazebo/set_model_state')
try:
set_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
resp = set_state(state_msg)
resp_targ = set_state(state_target_msg)
except rospy.ServiceException as e:
print("Service Call Failed: %s" % e)
initial_state = np.zeros(self.state_num)
self.move_cmd.linear.x = 0.0
self.move_cmd.angular.z = 0.0
self.pub.publish(self.move_cmd)
time.sleep(1)
self.pub.publish(self.move_cmd)
self.rate.sleep()
rospy.wait_for_service('phero_reset')
try:
phero_reset = rospy.ServiceProxy('phero_reset', PheroReset)
resp = phero_reset(True)
print("Reset Pheromone grid successfully: {}".format(resp))
except rospy.ServiceException as e:
print("Service Failed %s" % e)
return range(0, self.num_robots), initial_state
def _reset(self):
# Unpause simulation to make observation
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except (rospy.ServiceException) as e:
print("/gazebo/unpause_physics service call failed")
try:
# Wait for robot to be ready to accept signal
rospy.wait_for_message('motor_state', MotorSignal, timeout=5)
motor_signal = MotorSignal()
motor_signal.motor_type = 1
motor_signal.signal = [512, 512, 512, 512, 512, 512, 512, 512]
self.action_publisher.publish(motor_signal)
except:
print("cannot publish action")
time.sleep(.5)
# return to initial joint positions
rospy.wait_for_service('/gazebo/set_model_configuration')
try:
joint_names = [
'base_to_femur_1', 'base_to_femur_2', 'base_to_femur_3',
'base_to_femur_4', 'femur_to_tibia_1', 'femur_to_tibia_2',
'femur_to_tibia_3', 'femur_to_tibia_4'
]
joint_positions = [0, 0, 0, 0, 0, 0, 0, 0]
self.set_model_config_proxy('spyndra', 'spyndra_description',
joint_names, joint_positions)
except (rospy.ServiceException) as e:
print("/gazebo/set_model_configuration service call failed")
time.sleep(.5)
# return to initial model position
rospy.wait_for_service('/gazebo/set_model_state')
try:
model_state = ModelState()
model_state.model_name = 'spyndra'
model_state.pose.position.x, model_state.pose.position.y, model_state.pose.position.z = 0, 0, 0.5
model_state.pose.orientation.x, model_state.pose.orientation.y, model_state.pose.orientation.z, model_state.pose.orientation.w = 0, 0, 0, 0
model_state.twist.linear.x, model_state.twist.linear.y, model_state.twist.linear.z = 0, 0, 0
model_state.twist.angular.x, model_state.twist.angular.y, model_state.twist.angular.z = 0, 0, 0
self.set_model_state_proxy(model_state)
except (rospy.ServiceException) as e:
print("/gazebo/set_model_state service call failed")
time.sleep(1)
# stand up
try:
# Wait for robot to be ready to accept signal
rospy.wait_for_message('motor_state', MotorSignal, timeout=5)
motor_signal = MotorSignal()
motor_signal.motor_type = 1
motor_signal.signal = [512, 512, 512, 512, 820, 820, 820, 820]
self.action_publisher.publish(motor_signal)
except:
print("cannot publish action")
time.sleep(7)
# initiate observation
s_ = np.zeros(self.nS)
# older gazebo version always have different initial IMU info after reset, keep them zero
# # imu data update
# imu_data = None
# while imu_data is None:
# try:
# imu_data = rospy.wait_for_message('imu', Imu)
# s_[8: 14] = [imu_data.linear_acceleration.x, imu_data.linear_acceleration.y, imu_data.linear_acceleration.z, \
# imu_data.angular_velocity.x, imu_data.angular_velocity.y, imu_data.angular_velocity.z]
# except:
# pass
# motor data update
motor_data = None
while motor_data is None:
try:
motor_data = rospy.wait_for_message('motor_state', MotorSignal)
s_[:8] = motor_data.signal
except:
pass
# position data update
position_data = None
while position_data is None:
try:
position_data = rospy.wait_for_message('/gazebo/model_states',
ModelStates,
timeout=5)
index = position_data.name.index("spyndra")
#s_[8: 11] =
x, y, z = [
position_data.pose[index].position.x,
position_data.pose[index].position.y,
position_data.pose[index].position.y
]
self.INIT_POS = np.array([x, y, z])
self.GOAL = np.array(self.INIT_POS, copy=True)
self.GOAL[0] += 10
self.INIT_DIST = np.linalg.norm(self.INIT_POS - self.GOAL)
except:
pass
rospy.wait_for_service('/gazebo/pause_physics')
try:
#resp_pause = pause.call()
self.pause()
except (rospy.ServiceException) as e:
print("/gazebo/pause_physics service call failed")
self.START_TIME = time.time()
return s_
def main():
global regions_, position_, desired_position_, state_, yaw_, yaw_error_allowed_
global srv_client_go_to_point_, srv_client_wall_follower_
rospy.init_node('explorer1')
sub_laser = rospy.Subscriber('/uav1/scan', LaserScan, clbk_laser)
sub_odom = rospy.Subscriber('/uav1/ground_truth_to_tf', Odometry, clbk_odom)
#pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
rospy.wait_for_service('/go_to_point_switch1')
rospy.wait_for_service('/wall_follower_switch1')
rospy.wait_for_service('/gazebo/set_model_state')
srv_client_go_to_point_ = rospy.ServiceProxy('/go_to_point_switch1', SetBool)
srv_client_wall_follower_ = rospy.ServiceProxy('/wall_follower_switch1', SetBool)
srv_client_set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
# set robot position
model_state = ModelState()
model_state.model_name = 'uav1'
model_state.pose.position.x = initial_position_.x
model_state.pose.position.y = initial_position_.y
resp = srv_client_set_model_state(model_state)
# initialize going to the point
change_state(0)
rate = rospy.Rate(20)
while not rospy.is_shutdown():
if regions_ == None:
continue
if state_ == 0:
if regions_['front'] < 1.5 and regions_['fright'] <1.5 and regions_['fleft'] < 1.5 and regions_['right'] < 1.5 and regions_['left'] < 1.5:
change_state(1)
elif state_ == 1:
desired_yaw = math.atan2(desired_position_.y - position_.y, desired_position_.x - position_.x)
err_yaw = normalize_angle(desired_yaw - yaw_)
# less than 30 degrees
#if math.fabs(err_yaw) < (math.pi / 6) and \
#regions_['front'] > 1.5 and regions_['fright'] > 1.5 and regions_['fleft'] > 1.5:
#print 'less than 30'
#change_state(0)
# between 30 and 90
#if err_yaw > 0 and \
#math.fabs(err_yaw) > (math.pi / 6) and \
#math.fabs(err_yaw) < (math.pi / 2) and \
#regions_['left'] > 1.5 and regions_['fleft'] > 1:
#print 'between 30 and 90 - to the left'
#change_state(0)
#if err_yaw < 0 and \
#math.fabs(err_yaw) > (math.pi / 6) and \
#math.fabs(err_yaw) < (math.pi / 2) and \
#regions_['right'] > 1.5 and regions_['fright'] > 1:
#print 'between 30 and 90 - to the right'
#change_state(0)
if regions_['front'] > 2 and regions_['fright'] > 2 and regions_['fleft'] > 2 and regions_['right'] > .4 and regions_['left'] > .4:
change_state(0)
rate.sleep()
def moving(self):
state = 0
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]
if abs(obstacle_1.pose.position.x - 2) < 0.05 and abs(
obstacle_1.pose.position.y - 2) < 0.05:
state = 0
if state == 0:
obstacle_1.pose.position.x -= 0.005
obstacle_1.pose.position.y -= 0.012
if abs(obstacle_1.pose.position.x -
1.5) < 0.05 and abs(obstacle_1.pose.position.y -
0.8) < 0.05:
state = 1
elif state == 1:
obstacle_1.pose.position.x -= 0.01
obstacle_1.pose.position.y += 0.007
if abs(obstacle_1.pose.position.x -
0.5) < 0.05 and abs(obstacle_1.pose.position.y -
1.5) < 0.05:
state = 2
elif state == 2:
obstacle_1.pose.position.x -= 0.008
obstacle_1.pose.position.y -= 0.002
if abs(obstacle_1.pose.position.x +
0.3) < 0.05 and abs(obstacle_1.pose.position.y -
1.3) < 0.05:
state = 3
elif state == 3:
obstacle_1.pose.position.x -= 0.007
obstacle_1.pose.position.y -= 0.005
if abs(obstacle_1.pose.position.x + 1) < 0.05 and abs(
obstacle_1.pose.position.y - 0.8) < 0.05:
state = 4
elif state == 4:
obstacle_1.pose.position.x -= 0.01
obstacle_1.pose.position.y += 0.007
if abs(obstacle_1.pose.position.x + 2) < 0.05 and abs(
obstacle_1.pose.position.y - 1.5) < 0.05:
state = 5
elif state == 5:
obstacle_1.pose.position.x -= 0.002
obstacle_1.pose.position.y -= 0.007
if abs(obstacle_1.pose.position.x +
2.2) < 0.05 and abs(obstacle_1.pose.position.y -
0.8) < 0.05:
state = 6
elif state == 6:
obstacle_1.pose.position.x += 0.004
obstacle_1.pose.position.y -= 0.011
if abs(obstacle_1.pose.position.x +
1.8) < 0.05 and abs(obstacle_1.pose.position.y +
0.3) < 0.05:
state = 7
elif state == 7:
obstacle_1.pose.position.x += 0.003
obstacle_1.pose.position.y -= 0.007
if abs(obstacle_1.pose.position.x +
1.5) < 0.05 and abs(obstacle_1.pose.position.y +
1) < 0.05:
state = 8
elif state == 8:
obstacle_1.pose.position.x += 0.009
obstacle_1.pose.position.y += 0.007
if abs(obstacle_1.pose.position.x +
0.6) < 0.05 and abs(obstacle_1.pose.position.y +
0.3) < 0.05:
state = 9
elif state == 9:
obstacle_1.pose.position.x += 0.011
obstacle_1.pose.position.y -= 0.011
if abs(obstacle_1.pose.position.x -
0.5) < 0.05 and abs(obstacle_1.pose.position.y +
1.4) < 0.05:
state = 10
elif state == 10:
obstacle_1.pose.position.x += 0.006
obstacle_1.pose.position.y -= 0.006
if abs(obstacle_1.pose.position.x -
1.1) < 0.05 and abs(obstacle_1.pose.position.y +
2) < 0.05:
state = 11
elif state == 11:
obstacle_1.pose.position.x += 0.009
obstacle_1.pose.position.y += 0.01
if abs(obstacle_1.pose.position.x - 2) < 0.05 and abs(
obstacle_1.pose.position.y + 1) < 0.05:
state = 12
elif state == 12:
obstacle_1.pose.position.x -= 0.008
obstacle_1.pose.position.y += 0.01
if abs(obstacle_1.pose.position.x -
1.2) < 0.05 and abs(obstacle_1.pose.position.y +
0) < 0.05:
state = 13
elif state == 13:
obstacle_1.pose.position.x -= 0.007
obstacle_1.pose.position.y += 0.005
if abs(obstacle_1.pose.position.x -
0.5) < 0.05 and abs(obstacle_1.pose.position.y -
0.5) < 0.05:
state = 14
elif state == 14:
obstacle_1.pose.position.x += 0.017
obstacle_1.pose.position.y += 0.008
if abs(obstacle_1.pose.position.x -
2.2) < 0.05 and abs(obstacle_1.pose.position.y -
1.3) < 0.05:
state = 15
elif state == 15:
obstacle_1.pose.position.x -= 0.002
obstacle_1.pose.position.y += 0.007
if abs(obstacle_1.pose.position.x - 2) < 0.05 and abs(
obstacle_1.pose.position.y - 2) < 0.05:
state = 0
self.pub_model.publish(obstacle_1)
time.sleep(0.1)
def __init__(self,
queue_url,
aws_region='us-east-1',
race_duration=180,
number_of_trials=3,
number_of_resets=10000,
penalty_seconds=2.0,
off_track_penalty=2.0,
collision_penalty=5.0,
is_continuous=False,
race_type="TIME_TRIAL"):
# constructor arguments
self._model_updater = ModelUpdater.get_instance()
self._deepracer_path = rospkg.RosPack().get_path(
DeepRacerPackages.DEEPRACER_SIMULATION_ENVIRONMENT)
body_shell_path = os.path.join(self._deepracer_path, "meshes", "f1")
self._valid_body_shells = \
set(".".join(f.split(".")[:-1]) for f in os.listdir(body_shell_path) if os.path.isfile(
os.path.join(body_shell_path, f)))
self._valid_body_shells.add(const.BodyShellType.DEFAULT.value)
self._valid_car_colors = set(e.value for e in const.CarColorType
if "f1" not in e.value)
self._num_sectors = int(rospy.get_param("NUM_SECTORS", "3"))
self._queue_url = queue_url
self._region = aws_region
self._number_of_trials = number_of_trials
self._number_of_resets = number_of_resets
self._penalty_seconds = penalty_seconds
self._off_track_penalty = off_track_penalty
self._collision_penalty = collision_penalty
self._is_continuous = is_continuous
self._race_type = race_type
self._is_save_simtrace_enabled = False
self._is_save_mp4_enabled = False
self._is_event_end = False
self._done_condition = any
self._race_duration = race_duration
self._enable_domain_randomization = False
# sqs client
# The boto client errors out after polling for 1 hour.
self._sqs_client = SQSClient(queue_url=self._queue_url,
region_name=self._region,
max_num_of_msg=MAX_NUM_OF_SQS_MESSAGE,
wait_time_sec=SQS_WAIT_TIME_SEC,
session=refreshed_session(self._region))
self._s3_client = S3Client(region_name=self._region)
# tracking current state information
self._track_data = TrackData.get_instance()
self._start_lane = self._track_data.center_line
# keep track of the racer specific info, e.g. s3 locations, alias, car color etc.
self._current_racer = None
# keep track of the current race car we are using. It is always "racecar".
car_model_state = ModelState()
car_model_state.model_name = "racecar"
self._current_car_model_state = car_model_state
self._last_body_shell_type = None
self._last_sensors = None
self._racecar_model = AgentModel()
# keep track of the current control agent we are using
self._current_agent = None
# keep track of the current control graph manager
self._current_graph_manager = None
# Keep track of previous model's name
self._prev_model_name = None
self._hide_position_idx = 0
self._hide_positions = get_hide_positions(race_car_num=1)
self._run_phase_subject = RunPhaseSubject()
self._simtrace_video_s3_writers = []
self._local_model_directory = './checkpoint'
# virtual event only have single agent, so set agent_name to "agent"
self._agent_name = "agent"
# camera manager
self._camera_manager = CameraManager.get_instance()
# setting up virtual event top and follow camera in CameraManager
# virtual event configure camera does not need to wait for car to spawm because
# follow car camera is not tracking any car initially
self._main_cameras, self._sub_camera = configure_camera(
namespaces=[VIRTUAL_EVENT], is_wait_for_model=False)
self._spawn_cameras()
# pop out all cameras after configuration to prevent camera from moving
self._camera_manager.pop(namespace=VIRTUAL_EVENT)
dummy_metrics_s3_config = {
MetricsS3Keys.METRICS_BUCKET.value: "dummy-bucket",
MetricsS3Keys.METRICS_KEY.value: "dummy-key",
MetricsS3Keys.REGION.value: self._region
}
self._eval_metrics = EvalMetrics(
agent_name=self._agent_name,
s3_dict_metrics=dummy_metrics_s3_config,
is_continuous=self._is_continuous,
pause_time_before_start=PAUSE_TIME_BEFORE_START)
# upload a default best sector time for all sectors with time inf for each sector
# if there is not best sector time existed in s3
# use the s3 bucket and prefix for yaml file stored as environment variable because
# here is SimApp use only. For virtual event there is no s3 bucket and prefix past
# through yaml file. All are past through sqs. For simplicity, reuse the yaml s3 bucket
# and prefix environment variable.
virtual_event_best_sector_time = VirtualEventBestSectorTime(
bucket=os.environ.get("YAML_S3_BUCKET", ''),
s3_key=get_s3_key(os.environ.get("YAML_S3_PREFIX", ''),
SECTOR_TIME_S3_POSTFIX),
region_name=os.environ.get("APP_REGION", "us-east-1"),
local_path=SECTOR_TIME_LOCAL_PATH)
response = virtual_event_best_sector_time.list()
# this is used to handle situation such as robomaker job crash, so the next robomaker job
# can catch the best sector time left over from crashed job
if "Contents" not in response:
virtual_event_best_sector_time.persist(
body=json.dumps({
SECTOR_X_FORMAT.format(idx + 1): float("inf")
for idx in range(self._num_sectors)
}),
s3_kms_extra_args=utils.get_s3_kms_extra_args())
# ROS service to indicate all the robomaker markov packages are ready for consumption
signal_robomaker_markov_package_ready()
PhaseObserver('/agent/training_phase', self._run_phase_subject)
# setup mp4 services
self._setup_mp4_services()
def reset(self):
# rospy.wait_for_service('/gazebo/reset_world') # Reset simulation was causing problems, do not reset simulation
# try:
# #reset_proxy.call()
# self.reset_proxy()
# except (rospy.ServiceException) as e:
# print ("/gazebo/reset_world service call failed")
# rospy.wait_for_service('/iri_wam/controller_manager/list_controllers')
# try:
# readController = rospy.ServiceProxy('/iri_wam/controller_manager/list_controllers', Empty)
# control = readController()
# #print (control)
# #onHai = bool(controllers.iri_wam_controller.state == "stopped")
# except (rospy.ServiceException) as e:
# print ("Service call failed: %s"%e)
#if not onHai:
# rospy.wait_for_service('/iri_wam/controller_manager/switch_controller')
# try:
# change_controller = rospy.ServiceProxy('/iri_wam/controller_manager/switch_controller', SwitchController)
# ret = change_controller(['joint1_position_controller', 'joint2_position_controller', 'joint3_position_controller', 'joint4_position_controller', 'joint5_position_controller', 'joint6_position_controller', 'joint7_position_controller'], ['iri_wam_controller'], 2)
# except (rospy.ServiceException) as e:
# print ("Service call failed: %s"%e)
for joint in range(len(self.publishers)):
self.publishers[joint].publish(
self.home[joint]) #homing at every reset
self.open_gripper_func()
time.sleep(self.homingTime)
rospy.wait_for_service('/gazebo/set_model_state')
try:
pose = Pose()
state = ModelState()
state.model_name = self.objectName['obj1']
pose.position.x = 0.5001911647282589
pose.position.y = 0.1004797189877992
pose.position.z = 0.9
pose.orientation.x = 0.00470048637345294
pose.orientation.y = 0.99998892605584
pose.orientation.z = 9.419015715062839e-06
pose.orientation.w = -0.00023044483691539005
state.pose = pose
ret = self.set_state(state)
except (rospy.ServiceException) as e:
print("/gazebo/set model pose service call failed")
_, _ = self.get_object_pose(self.objectName['obj1'])
self.objInitial = self.object
self.objInitialJS = self.getInverseKinematics(
self.objInitial) # reference for sampling the goal
self.goal = self.sample_goal_onTable().copy(
) # get a random goal every time you reset
obs = self._get_obs()
rospy.wait_for_service('/gazebo/set_model_state')
try:
pose = Pose()
state = ModelState()
state.model_name = self.objectName['objFixed']
pose.position.x = self.goal[0]
pose.position.y = self.goal[1]
pose.position.z = 0.9
pose.orientation.x = 0.00470048637345294
pose.orientation.y = 0.99998892605584
pose.orientation.z = 9.419015715062839e-06
pose.orientation.w = -0.00023044483691539005
state.pose = pose
ret = self.set_state(state)
except (rospy.ServiceException) as e:
print("/gazebo/set model pose service call failed")
return obs
import utils #for teleop
'''EXPLICIT LOCATIONS USED'''
RED = [0.701716, 0.400784, 0.83095]
BLUE = [0.701716, 0.000784, 0.83095]
GREEN = [0.701716, -0.400784, 0.83095]
TARGET_GREEN = [0.6, -0.405, 0.82]
ALIGN_GRASP_GREEN = [0.7, -0.405, 0.82]
RAISED_GREEN = [0.7, -0.405, 1.0]
RAISED_DEST_GREEN = [0.7, 0.0, 1.0]
DEST_GREEN = [0.7, 0.0, 0.83]
DEST_GREEN_REMOVED = [0.45, 0.0, 1.0]
_cylinder_name = "unit_object_Green"
_cylinder_model_state = ModelState()
_cylinder_model_state.model_name = _cylinder_name
_cylinder_model_state.reference_frame = ''
'''OUTPUT FILE TO WRITE TO'''
FILENAME = '/home/simon/experiment'
def delete_cylinder():
rospy.wait_for_service("/gazebo/delete_model")
try:
s = rospy.ServiceProxy("gazebo/delete_model", DeleteModel)
resp = s(_cylinder_name)
except rospy.ServiceException, e:
print "error when calling delete_cylinder: %s"%e
sys.exit(1)
# print(position)
except:
time.sleep(1) # probably the publisher not started yet
pass
if __name__=="__main__":
rospy.init_node('human_controller')
pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=10)
print('Human Controller launched')
target_linear_vel = 0.0
target_angular_vel = 0.0
control_linear_vel = 0.0
control_angular_vel = 0.0
pose_subscriber = rospy.Subscriber('/gazebo/model_states', ModelStates, update_pose)
p1_state = ModelState()
p1_state.model_name = 'person_1'
rate = rospy.Rate(10)
t = 0
while not rospy.is_shutdown():
x_vel = 2*math.sin(t)
# x_vel = 2
ang_vel = 0.5
p1_state.twist.linear.x = x_vel
p1_state.twist.angular.x = ang_vel
pub.publish(p1_state)
t+=1
rate.sleep()
def image_callback(self, msg):
data = ModelState()
data.model_name = 'drone02'
drone = data.pose
drone_vel = data.twist
posX = drone.position.x
posY = drone.position.y
posZ = drone.position.z
velX = drone_vel.linear.x
velY = drone_vel.linear.y
#print(d01_posZ)
image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8') #bgr8
image = image[:, 30:image.shape[1]-30, :]
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
lower_flood = np.array([0, 100, 100])#np.array([100, 100, 0]) #yellow #np.array([0, 100, 100])
upper_flood = np.array([150, 255, 255])#np.array([255, 255, 120]) #yellow #np.array([100, 255, 255])
lower_ground = np.array([0, 0, 0])
upper_ground = np.array([110, 110, 110])
mask_flood = cv2.inRange(hsv, lower_flood, upper_flood)
mask_ground = cv2.inRange(hsv, lower_ground, upper_ground)
h, w, d = image.shape
#print(h,w)
search_top = 0#3*h/4
search_bot = h#3*h/4 + 20
mask_flood[0:search_top, 0:w] = 0
mask_flood[search_bot:h, 0:w] = 0
mask_ground[0:search_top, 0:w] = 0
mask_ground[search_bot:h, 0:w] = 0
M_flood = cv2.moments(mask_flood)
M_ground = cv2.moments(mask_ground)
if M_flood['m00'] > 0 and M_ground['m00'] > 0:
print('fine')
cx_flood = int(M_flood['m10']/M_flood['m00'])
cy_flood = int(M_flood['m01']/M_flood['m00'])
cv2.circle(image, (cx_flood, cy_flood), 10, (0,0,255), -1)
cx_ground = int(M_ground['m10']/M_ground['m00'])
cy_ground = int(M_ground['m01']/M_ground['m00'])
cv2.circle(image, (cx_ground, cy_ground), 10, (255,0,0), -1)
cx_middle = min(cx_flood, cx_ground) + int((max(cx_flood, cx_ground)-min(cx_flood, cx_ground))/2)
cy_middle = min(cy_flood, cy_ground) + int((max(cy_flood, cy_ground)-min(cy_flood, cy_ground))/2)
cv2.circle(image, (cx_middle, cy_middle), 10, (0,0,0), -1)
cv2.line(image, (cx_flood, cy_flood), (cx_ground, cy_ground), (0,255,0), 1)
# BEGIN CONTROL
err_x = cx_middle - w/2
err_y = cy_middle - h/2
#print("err_x: ", err_x, "err_y: ", err_y)
self.twist.linear.x = -float(err_y)/300.0 - 1.0
self.twist.linear.y = -float(err_x)/85.0
self.twist.linear.z = 0 #-1.1
#vel_x = -float(err_y)/300.0 + 1.0
#vel_y = -float(err_x)/85.0
self.twist.angular.z = -float(math.atan2((cy_flood - cy_ground),(cx_flood - cx_ground)))*1.8
self.cmd_vel_pub.publish(self.twist)
elif M_flood['m00'] > 0 and M_ground['m00'] <= 0 and M_ground['m10'] <= 0:
print('lost_ground')
cx_flood = int(M_flood['m10']/M_flood['m00'])
cy_flood = int(M_flood['m01']/M_flood['m00'])
cv2.circle(image, (cx_flood, cy_flood), 10, (0,0,255), -1)
self.twist.linear.x = 0.0
self.twist.linear.y = 0.0
self.twist.linear.z = 0.8
self.cmd_vel_pub.publish(self.twist)
elif M_flood['m00'] <= 0 and M_flood['m10'] <= 0 and M_ground['m00'] > 0:
print('lost_flood')
cx_ground = int(M_ground['m10']/M_ground['m00'])
cy_ground = int(M_ground['m01']/M_ground['m00'])
cv2.circle(image, (cx_ground, cy_ground), 10, (255,0,0), -1)
self.twist.linear.x = 0.0
self.twist.linear.y = 0.0
self.twist.linear.z = 0.8
self.cmd_vel_pub.publish(self.twist)
else:
self.twist.linear.x = 0.0
self.twist.linear.y = 0.0
self.twist.linear.z = 0.0
self.twist.angular.z = 0.0
self.cmd_vel_pub.publish(self.twist)
print("else")
#cv2.imshow("mask_flood",mask_flood)
#cv2.imshow("mask_ground", mask_ground)
cv2.imshow("d2", image)
cv2.waitKey(3)
def set_target(self):
self.goal_pose.position.x = random.uniform(-3.6, 3.6)
self.goal_pose.position.y = random.uniform(-3.6, 3.6)
target_state = ModelState(model_name="target", pose=self.goal_pose)
self.set_model_state(target_state)
turn = turn * speedBindings[key][1]
count = 0
print vels(speed, turn)
if status == 14:
print msg
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':
def __init__(self):
"""
This Task Env is designed for having the TurtleBot2 in some kind of maze.
It will learn how to move around the maze without crashing.
"""
# This is the path where the simulation files, the Task and the Robot gits will be downloaded if not there
ros_ws_abspath = rospy.get_param("/turtlebot2/ros_ws_abspath", None)
assert ros_ws_abspath is not None, "You forgot to set ros_ws_abspath in your yaml file of your main RL script. Set ros_ws_abspath: \'YOUR/SIM_WS/PATH\'"
assert os.path.exists(ros_ws_abspath), "The Simulation ROS Workspace path " + ros_ws_abspath + \
" DOESNT exist, execute: mkdir -p " + ros_ws_abspath + \
"/src;cd " + ros_ws_abspath + ";catkin_make"
ROSLauncher(rospackage_name="turtlebot_gazebo",
launch_file_name="start_goal_world.launch",
ros_ws_abspath=ros_ws_abspath)
# Load Params from the desired Yaml file
LoadYamlFileParamsTest(
rospackage_name="openai_ros",
rel_path_from_package_to_file=
"src/openai_ros/task_envs/turtlebot2/config",
yaml_file_name="turtlebot2_goal_continuous_humanmodel.yaml")
# Here we will add any init functions prior to starting the MyRobotEnv
super(TurtleBot2HumanModelEnv, self).__init__(ros_ws_abspath)
# Only variable needed to be set here
high = numpy.array([1, 1, 1, 1])
low = numpy.array([-1, -1, -1, -1])
#high = numpy.array([0,1,1,1])
#low = numpy.array([0,-1,-1,-1])
self.action_space = spaces.Box(low, high)
# We set the reward range, which is not compulsory but here we do it.
self.reward_range = (-numpy.inf, numpy.inf)
self.success = True
self.beta = 0
#self.beta = 1
#number_observations = rospy.get_param('/turtlebot2/n_observations')
"""
We set the Observation space for the 6 observations
cube_observations = [
round(current_disk_roll_vel, 0),
round(y_distance, 1),
round(roll, 1),
round(pitch, 1),
round(y_linear_speed,1),
round(yaw, 1),
]
"""
# Actions and Observations
self.init_linear_forward_speed = rospy.get_param(
'/turtlebot2/init_linear_forward_speed')
self.init_linear_turn_speed = rospy.get_param(
'/turtlebot2/init_linear_turn_speed')
self.new_ranges = 180
self.min_range = rospy.get_param('/turtlebot2/min_range')
self.max_laser_value = rospy.get_param('/turtlebot2/max_laser_value')
self.min_laser_value = rospy.get_param('/turtlebot2/min_laser_value')
# Get Desired Point to Get
self.desired_point = Point()
self.desired_point.x = rospy.get_param("/turtlebot2/desired_pose/x")
self.desired_point.y = rospy.get_param("/turtlebot2/desired_pose/y")
self.desired_point.z = rospy.get_param("/turtlebot2/desired_pose/z")
self.state_msg = ModelState()
self.state_msg.model_name = 'mobile_base'
self.state_msg.pose.position.x = 0
self.state_msg.pose.position.y = 0
self.state_msg.pose.position.z = 0
self.state_msg.pose.orientation.x = 0
self.state_msg.pose.orientation.y = 0
self.state_msg.pose.orientation.z = 0
self.state_msg.pose.orientation.w = 0
# We create two arrays based on the binary values that will be assigned
# In the discretization method.
laser_scan = self.get_laser_scan()
#print("lidar data:", laser_scan)
rospy.logdebug("laser_scan len===>" + str(len(laser_scan.ranges)))
#high = numpy.array([0.5,1,1,1,1,1,6,3.14])#,numpy.array([12,6,3.14,1,3.14,0.5,1]),6*numpy.ones([self.new_ranges]),numpy.array([12,6,3.14,1,3.14,0.5,1]),6*numpy.ones([self.new_ranges])))
high = numpy.hstack(
(numpy.array([0.5, 1, 0.5, 1]), 6 * numpy.ones([self.new_ranges])))
#high = numpy.hstack((numpy.array([0,1,0.5,1]),6*numpy.ones([self.new_ranges])))
#high = numpy.hstack((numpy.array([1,1]),numpy.ones([self.new_ranges]),numpy.array([1,1]),numpy.ones([self.new_ranges]),numpy.array([1,1]),numpy.ones([self.new_ranges])))
#low = numpy.array([-0.5,-1,-1,-1,-1,-1, 0,-3.14])#,numpy.array([-1,-1*6,-1*3.14,-1,-3.14,-0.5,-1]),numpy.zeros([self.new_ranges]),numpy.array([-1,-1*6,-1*3.14,-1,-3.14,-0.5,-1]),numpy.zeros([self.new_ranges])))
low = numpy.hstack((numpy.array([-0.5, -1, -0.5,
-1]), numpy.zeros([self.new_ranges])))
#low = numpy.hstack((numpy.array([0,-1,-0.5,-1]),numpy.zeros([self.new_ranges])))
#low = numpy.hstack((numpy.array([-1,-1]),numpy.zeros([self.new_ranges]),numpy.array([1,1]),numpy.ones([self.new_ranges]),numpy.array([1,1]),numpy.ones([self.new_ranges])))
# We only use two integers
self.observation_space = spaces.Box(low, high)
rospy.logdebug("ACTION SPACES TYPE===>" + str(self.action_space))
rospy.logdebug("OBSERVATION SPACES TYPE===>" +
str(self.observation_space))
# Rewards
self.forwards_reward = rospy.get_param("/turtlebot2/forwards_reward")
self.turn_reward = rospy.get_param("/turtlebot2/turn_reward")
self.end_episode_points = rospy.get_param(
"/turtlebot2/end_episode_points")
self.cumulated_steps = 0.0
############################## goal ##############################################
self.goal_position = Pose()
self.f = open(
'/home/i2rlab/shahil_files/shahil_RL_ws_new/src/turtlebot/turtlebot_gazebo/worlds/goal/model.sdf',
'r')
self.sdff = self.f.read()
############################## Obstacle ##########################################
self.angle = numpy.linspace(-179, 179, 180) / 180 * numpy.pi
self.cos = numpy.cos(self.angle)
self.sin = numpy.sin(self.angle)
############################## Human Model ######################################
config = tf.ConfigProto(gpu_options=tf.GPUOptions(allow_growth=True))
self.sess = tf.Session(config=config)
self.S1 = tf.placeholder(tf.float32, [None, 5], 'S1')
#self.S2 = tf.placeholder(tf.float32, [None, 198, 1], 'S2')
self.S2 = tf.placeholder(tf.float32, [None, 180], 'S2')
self.keep_prob = tf.placeholder(tf.float32)
self.a_predict = self.build_c(self.S1, self.S2, self.keep_prob)
self.loader()
self.goal_space()
self.time_start = 0
#!/usr/bin/env python
import rospy
import tf
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
from gazebo_msgs.msg import ModelState
from gazebo_msgs.srv import SetModelState
rospy.init_node('box')
pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=5)
quaternion = tf.transformations.quaternion_from_euler(0.000338, 0.003608,
0.000875)
msg = ModelState()
msg.model_name = 'robot1'
msg.pose.position.x = -2.873680
msg.pose.position.y = -0.128694
msg.pose.position.z = 0.012142
msg.pose.orientation.x = quaternion[0]
msg.pose.orientation.y = quaternion[1]
msg.pose.orientation.z = quaternion[2]
msg.pose.orientation.w = quaternion[3]
msg.reference_frame = 'world'
rate = rospy.Rate(50)
try:
set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
resp = set_state(msg)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def run(self, args):
commands = args.commands
# Split into commands and arguments, every other index
arguments = commands[1::2]
commands = commands[0::2]
for i in xrange(len(commands)):
command = commands[i]
argument = arguments[i]
self.send_feedback("Waiting for odom...")
yield self.tx_pose()
self.send_feedback("Odom found!")
action_kwargs = {
'speed': float(args.speed),
'blind': bool(args.blind)
}
if command == 'custom':
# Let the user input custom commands, the eval may be dangerous
# so do away with that at some point.
self.send_feedback(
"Moving with the command: {}".format(argument))
res = yield eval("self.move.{}.go()".format(
argument, **action_kwargs))
elif command in ['tp', 'teleport']:
try:
rospack = rospkg.RosPack()
state_set_pub = yield self.nh.advertise(
'gazebo/set_model_state', ModelState)
config_file = os.path.join(rospack.get_path('sub8_gazebo'),
'config', 'teleport_locs.yaml')
f = yaml.load(open(config_file, 'r'))
if len(arguments) > 1:
# Command only takes in one string so to prevent this
# command from flowing over into movement we break
# before it proceeds.
self.send_feedback(
"Error, more than one argument detected.")
break
else:
try:
x = float(argument.split(' ')[0])
y = float(argument.split(' ')[1])
z = float(argument.split(' ')[2])
# Assumption is if we make it this far, we have successfully
# bound the previous three coordinates.
# The below would fail if we entered a location name instead of coords
# but we should have caught by this point.
# This is to catch anything over 3 coordinates. If
# only two were given then we would also error out
# above.
if len(argument.split(' ')) != 3:
self.send_feedback(
"Incorrect number of coordinates")
break
except IndexError:
self.send_feedback(
"Incorrect number of coordinates")
break
except ValueError:
try:
if argument in ('list', 'l'):
self.send_feedback(
'Available TP locations:')
for k in f:
print ' * ' + k
break
argz = f[argument]
x = float(argz.split(' ')[0])
y = float(argz.split(' ')[1])
z = float(argz.split(' ')[2])
except LookupError:
# This means we did not find the saved location
# referenced by the argument.
self.send_feedback(
"TP location not found, check input.")
break
modelstate = ModelState(
model_name='sub8',
pose=Pose(position=Point(x, y, z),
orientation=Quaternion(0, 0, 0, 0)),
twist=Twist(linear=Point(0, 0, 0),
angular=Point(0, 0, 0)),
reference_frame='world')
# Sometimes you need to sleep in order to get the thing to publish
# Apparently there is a latency when you set a publisher and it needs to actually hook into it.
# As an additional note, given the way we do trajectory in the sim, we must kill sub to prevent
# the trajectory from overriding our teleport and
# bringing it back to its expected position.
ab = TxAlarmBroadcaster(self.nh,
'kill',
node_name='kill')
yield ab.raise_alarm(
problem_description='TELEPORTING: KILLING SUB',
severity=5)
yield self.nh.sleep(1)
yield state_set_pub.publish(modelstate)
yield self.nh.sleep(1)
yield ab.clear_alarm()
except KeyboardInterrupt:
# Catches a ctrl-c situation and ends the program. Break
# will just bring the program to its natural conclusion.
break
elif command in ['zrp', 'level_off', 'zpr']:
self.send_feedback("Zeroing roll and pitch")
res = yield self.move.zero_roll_and_pitch().go(**action_kwargs)
elif command == "stop":
self.send_feedback("Stopping...")
if args.zrp:
res = yield self.move.forward(0).zero_roll_and_pitch().go(
**action_kwargs)
else:
res = yield self.move.forward(0).go(**action_kwargs)
else:
# Get the command from shorthand if it's there
command = SHORTHAND.get(command, command)
movement = getattr(self.move, command)
trans_move = command[:
3] != "yaw" and command[:
5] != "pitch" and command[:
4] != "roll"
unit = "m" if trans_move else "rad"
amount = argument
# See if there's a non standard unit at the end of the argument
if not argument[-1].isdigit():
last_digit_index = [
i for i, c in enumerate(argument) if c.isdigit()
][-1] + 1
amount = argument[:last_digit_index]
unit = argument[last_digit_index:]
extra = "and zeroing pitch and roll" if args.zrp else ""
self.send_feedback("{}ing {}{} {}".format(
command, amount, unit, extra))
bad_unit = UNITS.get(unit, "BAD")
if bad_unit == "BAD":
self.send_feedback("BAD UNIT")
break
goal = movement(float(amount) * UNITS.get(unit, 1))
if args.zrp:
res = yield goal.zero_roll_and_pitch().go(**action_kwargs)
else:
res = yield goal.go(**action_kwargs)
self.send_feedback("Result: {}".format(res.error))
def main():
global regions_, position_, desired_position_, state_, yaw_, yaw_error_allowed_
global srv_client_go_to_goal_, srv_client_wall_follower_
global circumnavigate_closest_point_, circumnavigate_starting_point_
global count_loop_, count_state_time_
rospy.init_node('bug1_algorithm')
sub_laser = rospy.Subscriber('/m2wr/laserscan', LaserScan, laser_callback)
sub_odom = rospy.Subscriber('/odom', Odometry, odom_callback)
rospy.wait_for_service('/go_to_goal_switch')
rospy.wait_for_service('/right_wall_following_switch')
rospy.wait_for_service('/gazebo/set_model_state')
srv_client_go_to_goal_ = rospy.ServiceProxy('/go_to_goal_switch', SetBool)
srv_client_wall_follower_ = rospy.ServiceProxy(
'/right_wall_following_switch', SetBool)
srv_client_set_model_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
# Set robot position
model_state = ModelState()
model_state.model_name = 'm2wr'
model_state.pose.position.x = initial_position_.x
model_state.pose.position.y = initial_position_.y
resp = srv_client_set_model_state(model_state)
# Initialize going to the point
change_state(0)
rate = rospy.Rate(20)
while not rospy.is_shutdown():
if regions_ == None:
continue
# 0 - go to point
if state_ == 0:
if regions_['front'] > 0.15 and regions_['front'] < 1:
circumnavigate_closest_point_ = position_
circumnavigate_starting_point_ = position_
change_state(1)
# 1 - circumnavigate
elif state_ == 1:
# If current position is closer to the goal than the previous closest_position, assign current position to closest_point
if calc_dist_points(
position_, desired_position_) < calc_dist_points(
circumnavigate_closest_point_, desired_position_):
circumnavigate_closest_point_ = position_
# rospy.loginfo("New closest point update: %s " % circumnavigate_closest_point_)
# Compare only after 5 seconds - need some time to get out of starting_point
# If robot reaches (is close to) starting point
if count_state_time_ > 5 and \
calc_dist_points(position_, circumnavigate_starting_point_) < 0.2:
change_state(2)
# 2 - go to closest point
elif state_ == 2:
# If robot reaches (is close to) closest point
if calc_dist_points(position_,
circumnavigate_closest_point_) < 0.2:
change_state(0)
count_loop_ = count_loop_ + 1
if count_loop_ == 20:
count_state_time_ = count_state_time_ + 1
count_loop_ = 0
rate.sleep()
rospy.wait_for_service('gazebo/pause_physics')
pause_physics = rospy.ServiceProxy('gazebo/pause_physics', Empty)
rospy.wait_for_service('gazebo/unpause_physics')
unpause_physics = rospy.ServiceProxy('gazebo/unpause_physics', Empty)
print "got service 'gazebo/set_model_state'"
for idx, location in enumerate(tqdm(availableLoc)):
for plan in location:
pose = plan
time_start = time.time()
state = ModelState(model_name="sudo-spawn",
pose=pose,
reference_frame="map")
pause_physics()
response = setModelState(state)
time.sleep(0.1)
unpause_physics()
time.sleep(0.2)
image_data = rospy.wait_for_message(
"/sudo/bottom_webcam/image_raw", Image)
image = cvBridge.imgmsg_to_cv2(image_data, "bgr8")
cv2.imshow("AAAAAARGH", image)
cv2.waitKey(1)
orientation = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
yaw = tf.transformations.euler_from_quaternion(orientation)[2]
total_angle = math.degrees(yaw)
if __name__ == "__main__":
rospy.init_node("model_dynamics")
model = bicycleModel()
# fusion = sensorFusion()
endList = 0;
print(model.waypointList)
#wait till a waypoint is received
while(not model.waypointList):
pass
rospy.sleep(7)
targetState = ModelState()
# targetState = model.waypointList.pop(0)
# print("target state", targetState)
targetState.pose.position.x = 10
targetState.pose.position.y = 10
safe = True
start = time.time()
rate = rospy.Rate(100) # 100 Hz
while not rospy.is_shutdown():
rate.sleep() # Wait a while before trying to get a new state
currState = model.getModelState()
if not currState.success:
continue
vy = msg.axes[4]
if not msg.buttons[6] and msg.buttons[7]:
vz = 1
elif msg.buttons[6] and not msg.buttons[7]:
vz = -1
else:
vz = 0
rospy.init_node('joystick', anonymous=True)
swarm_pub = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=1000)
rospy.Subscriber('/joy', Joy, joy_callback)
swarm_msg = [ModelState() for _ in xrange(25)]
for i in xrange(25):
swarm_msg[i].model_name = 'quadrotor' + str(i)
dt = 0.1
x = 0
y = 0
z = 0
while not rospy.is_shutdown():
x = x + vx * dt
y = y + vy * dt
z = z + vz * dt
q = quaternion_from_euler(-0.25 * vy, 0.25 * vx, 0)
def respawn(self):
self.counter=0
while not rospy.is_shutdown():
self.spawn_publisher = rospy.Publisher("/gazebo/set_model_state", ModelState, queue_size=1)
self.counter+=1
rospy.sleep(0.0002)
print self.is_episode_finished()
print self.linear_velocity,self.counter
print self.angular_velocity
# print counter
if self.counter<20000:
posex = self.odom_data[0][0]
posey = self.odom_data[0][1]
posez = self.odom_data[0][2]
poseox = self.odom_data[1][0]
poseoy = self.odom_data[1][1]
poseoz = self.odom_data[1][2]
poseow = self.odom_data[1][3]
twistx = self.linear_velocity[0]
twisty = self.linear_velocity[1]
twistz = self.linear_velocity[2]
twistax = self.angular_velocity[0]
twistay = self.angular_velocity[1]
twistaz = self.angular_velocity[2]
if self.counter>20000:
if 20000<=self.counter<24000:
posex = self.odom_data[0][0]
posey = self.odom_data[0][1]
posez = self.odom_data[0][2]
poseox = self.odom_data[1][0]
poseoy = self.odom_data[1][1]
poseoz = self.odom_data[1][2]
poseow = self.odom_data[1][3]
twistx = self.twist_data[0][0]
twisty = self.twist_data[0][1]
twistz = self.twist_data[0][2]
twistax = self.twist_data[1][0]
twistay = self.twist_data[1][1]
twistaz = self.twist_data[1][2]
# print self.state
elif 24000<=self.counter<25500:
posex = 44.2
posey = -100
posez = 0.3
poseox = 0
poseoy = 0
poseoz = 0
poseow = 1
twistx = 0
twisty = 0
twistz = 0
twistax = 0
twistay = 0
twistaz = 0
# rospy.loginfo("########### Respawning the car... ###########")
if self.counter >= 25500:
# self.counter = 0
self.new_episode()
states = ModelState()
states.model_name = "vehicle"
states.twist.linear.x = twistx
states.twist.linear.y = twisty
states.twist.linear.z = twistz
states.twist.angular.x = twistax
states.twist.angular.y = twistay
states.twist.angular.z = twistaz
states.pose.position.x = posex
states.pose.position.y = posey
states.pose.position.z = posez
states.pose.orientation.x = poseox
states.pose.orientation.y = poseoy
states.pose.orientation.z = poseoz
states.pose.orientation.w = poseow
self.spawn_publisher.publish(states)
def parse_trajectory(input_file):
# Extract the start position of the robot
with open(input_file, 'r') as input:
lines = input.readlines()
start_position = [int(lines[1].split()[1]), int(lines[2].split()[1])]
# Used to keep track of the top of a reading chunk
reading_num = 4
# read every chunk of data
while reading_num < len(lines) - CHUNK_LEN:
# Append the heading and distance to the heading_distance_list
heading_words = lines[reading_num].split()
distance_words = lines[reading_num + 1].split()
heading = Float32(heading_words[2])
distance = Float32(distance_words[2])
heading_distance_list.append([heading, distance])
# Reading ground truth data
# Assign Pose
pose = Pose()
# position
pose.position.x = Float64(lines[reading_num + 6].split()[1])
pose.position.y = Float64(lines[reading_num + 7].split()[1])
pose.position.z = Float64(lines[reading_num + 8].split()[1])
# orientation
pose.orientation.x = Float64(lines[reading_num + 10].split()[1])
pose.orientation.y = Float64(lines[reading_num + 11].split()[1])
pose.orientation.z = Float64(lines[reading_num + 12].split()[1])
pose.orientation.w = Float64(lines[reading_num + 13].split()[1])
# Assign Twist
twist = Twist()
# linear
twist.linear.x = Float64(lines[reading_num + 16].split()[1])
twist.linear.y = Float64(lines[reading_num + 17].split()[1])
twist.linear.z = Float64(lines[reading_num + 18].split()[1])
# angular
twist.angular.x = Float64(lines[reading_num + 20].split()[1])
twist.angular.y = Float64(lines[reading_num + 21].split()[1])
twist.angular.z = Float64(lines[reading_num + 22].split()[1])
# Append ground truth to list
model_state = ModelState()
model_state.pose = pose
model_state.twist = twist
ground_truth_list.append(model_state)
# Append noisy headings and distances to their list
noisy_heading_distance_list.append([
Float32(lines[reading_num + 25].split()[1].replace(
"[", "").replace(",", "")),
Float32(lines[reading_num + 27].split()[1].replace(
"[", "").replace(",", ""))
])
# Append scan data to list
scan_data = lines[reading_num + 29][11:].split()
scan_data_list = TurtleBotScan().ranges
for data in scan_data:
scan_data_list.append(
Float32(data.replace(",", "").replace("]", "")))
scan_list.append(scan_data_list)
reading_num = reading_num + 30
return start_position, heading_distance_list, ground_truth_list, noisy_heading_distance_list, scan_list
def publish_gazebo_model_state(self):
for i in range(0, len(self.vect_gazebo)):
try:
position_to_pub = ModelState()
# is the mark visible? if not we reach exception
# (trans_to_pub, rot_to_pub) = self.listener.lookupTransform(
# self.vect_gazebo[i][1], MAP, rospy.Time(0))
# Where is the desired object
(trans_to_pub, rot_to_pub) = self.listener.lookupTransform(
MAP, self.vect_gazebo[i][0], rospy.Time(0))
# create the special message for gazebo
position_to_pub.reference_frame = "world"
position_to_pub.pose.position.x = trans_to_pub[0]
position_to_pub.pose.position.y = trans_to_pub[1]
# TODO : this is to be sure that the table is on the ground
# but it can not be generalized because the chair origin
# is not on the ground, we have to decide wether to fix
# the origin of all objects gazebo on the ground or pass
# the Z in a parameter or just the marker info
if self.vect_gazebo[i][1] == "table":
quat_to_send = rot_to_pub
position_to_pub.pose.position.z = 0
elif i == 0:
euler = euler_from_quaternion(rot_to_pub)
quat_to_send = quaternion_from_euler(
math.pi - euler[2], 0, 0)
position_to_pub.pose.position.z = trans_to_pub[2]
else:
quat_to_send = rot_to_pub
position_to_pub.pose.position.z = trans_to_pub[2]
position_to_pub.pose.orientation.x = quat_to_send[0]
position_to_pub.pose.orientation.y = quat_to_send[1]
position_to_pub.pose.orientation.z = quat_to_send[2]
position_to_pub.pose.orientation.w = quat_to_send[3]
if i == 0:
position_to_pub.model_name = "/virtual_pepper"
self.pub.publish(position_to_pub)
(trans_to_pub, rot_to_pub) = self.listener.lookupTransform(
MAP, "/base_link", rospy.Time(0))
# self.broadcaster.sendTransform(
# (0, 0, 0), (0, 0, 0, 1), rospy.Time.now(),
# "/map", "/world")
self.broadcaster.sendTransform(
trans_to_pub, rot_to_pub, rospy.Time.now(),
"/virtual_pepper/base_link", MAP)
else:
position_to_pub.model_name = self.vect_gazebo[i][0]
self.pub_obj.publish(position_to_pub)
except Exception, exc:
print"publish gazebo", exc
rospy.set_param('/evaluate',evaluate)
# 4.2.1b Reset environment ==> causes gt_node to freeze for more than a minute...
# reset_gazebo_service(EmptyRequest())
# 4.2.1c Change position of drone according to new selected starting position
pose=Pose()
pose.position.x, pose.position.y, starting_height, yaw = sample_new_position(starting_positions)
# pose.position.x, pose.position.y, starting_height, yaw=0,0,1,0
print("[run_script]: x: {0}, y: {1}, z: {2}, yaw:{3}".format(pose.position.x, pose.position.y, starting_height, yaw))
# some yaw to quaternion re-orientation code:
pose.orientation.z=np.sin(yaw)
pose.orientation.w=np.cos(yaw)
pose.position.z = 0.1
model_state = ModelState()
model_state.model_name = 'quadrotor' if FLAGS.robot.startswith('drone') else 'turtlebot3_burger'
model_state.pose=pose
state_request = SetModelStateRequest()
state_request.model_state = model_state
retvals = model_state_gazebo_service(state_request)
rospy.set_param('starting_height', starting_height)
print("Changed pose with return values: {0}".format(retvals))
time.sleep(1) #CHANGED
unpause_physics_client(EmptyRequest())
else:
# 4.2.2 Launch Gazebo again
# 4.2.2a Ensure previous Gazebo is not running anymore
if gazebo_popen!=None:
def simulate_anomaly():
"""
Spawn and move the fire
"""
# {id: [ initial_pos, speed]}
anomalies = {'fogo4': [[-0.50, 0.30], [0.0051, 0.0]],
'fogo5': [[-0.4, -0.30], [0.0051, 0.0]],
'fogo6': [[-0.65, 0.2], [0.0051, 0.0]],
'fogo7': [[0.5, 0], [0.0051, 0.0]],
'fogo8': [[0, 0.4], [0.0051, 0.0]],
'fogo9': [[0, -0.35], [0.0051, 0.0]],
'fogo1': [[0.40, 0.40], [0.0051, 0.0]],
'fogo2': [[-2.30, 0.20], [0.0051, 0.0]],
}
# {id: initial_position, target, initial time}
anomalies = {'fogo4': [[-0.50, 0.30], [5.00, 0.0]],
'fogo5': [[-0.4, -1.60], [5.00, 0.0]],
'fogo6': [[-1.0, -2.02], [5.00, 0.0]],
'fogo7': [[1.0, -0.30], [5.00, 0.0]],
'fogo8': [[0.0, 0.4], [5.00, 0.0]],
'fogo9': [[-1.30, -0.90], [5.00, 0.0]],
'fogo1': [[0.80, -1.60], [5.00, 0.0]],
'fogo2': [[-0.8, 0.0], [5.00, 0.0]],
}
anomalies = {'fogo4': [[-0.50, 0.30], [.00, 0.0], 0],
'fogo5': [[-0.4, -1.60], [.00, 0.0], 0],
'fogo6': [[-1.0, -2.02], [.00, 0.0], 0],
'fogo7': [[1.0, -0.30], [.00, 0.0], 0],
'fogo8': [[0.0, 0.4], [.00, 0.0], 0],
'fogo9': [[-1.30, -0.90], [.00, 0.0], 0],
'fogo1': [[0.80, -1.60], [.00, 0.0], 0],
'fogo2': [[-0.8, 0.0], [.00, 0.0], 0],
}
anomalies = {'fogo4': [[-4.0, 3.0], [-4.0, 3.0], 40],
'fogo5': [[1.4, 1.80], [1.4, 1.80], 100],
'fogo6': [[-2.60, -1.2], [-2.60, -1.2], 150],
# 'fogo7': [[3.30, -2.40], [3.30, -2.40], 200]
}
anomalies = {'fogo4': [[-.0, .0], [-.0, .0], 0],
}
existent_anomalies = Set()
expand = rospy.get_param('~expand', False)
if expand:
for k, a in anomalies.items():
anomalies[k][0], anomalies[k][1] = anomalies[k][1], anomalies[k][0]
communicator = Communicator('Robot1')
# ## SPAWN
# for anom, data in anomalies.items():
# spawn_fire(anom, data[0][0], data[0][1])
# pass
# ### MOVE
service_name = '/gazebo/set_model_state'
rospy.init_node('anomaly_simulator')
rospy.wait_for_service(service_name)
client_ms = rospy.ServiceProxy(service_name, SetModelState)
rospy.sleep(0.1)
r = rospy.Rate(2) # 10hz
#
# # moving_x = 0
log = []
while not rospy.is_shutdown():
for anom, data in anomalies.items():
if data[2] > rospy.get_rostime().secs:
continue
else:
if not anom in existent_anomalies:
print "spawn fire ", anom
spawn_fire(anom, data[0][0], data[0][1])
existent_anomalies.add(anom)
x, y = data[0]
target_x, target_y = data[1]
angle = math.atan2(target_y - y, target_x - x)
v = 0.002
data[0][0] += v * math.cos(angle)
data[0][1] += v * math.sin(angle)
# Model state
ms = ModelState()
ms.model_name = anom
ms.pose.position.x, ms.pose.position.y = data[0]
ms.reference_frame = 'world'
# Set new model state
set_ms = SetModelState()
set_ms.model_state = ms
client_ms.call(ms)
rospy.sleep(0.05)
time = rospy.get_rostime().secs + rospy.get_rostime().nsecs / 1000000000.0
orobots, sensed_points, anomaly_polygons = communicator.read_inbox()
log.append((time, anomaly_polygons, copy.deepcopy(anomalies), sensed_points))
with open(LOG_FILE, 'wb') as output:
pickle.dump(log, output, pickle.HIGHEST_PROTOCOL)
r.sleep()