def reset(self):
print "reset"
rospy.wait_for_service('/gazebo/reset_world')
try:
reset_world()
except(rospy.ServiceException) as e:
print "reset_world failed!"
rospy.wait_for_service('/gazebo/reset_simulation')
try:
reset_simulation()
except(rospy.ServiceException) as e:
print "reset_simulation failed!"
reset_cube = ModelState()
reset_cube.model_name = "cube2"
reset_cube.reference_frame = "world"
reset_cube.pose.position.x = 0
reset_cube.pose.position.y = 0
reset_cube.pose.position.z = 0
reset_cube.pose.orientation.x = 0
reset_cube.pose.orientation.y = 0
reset_cube.pose.orientation.z = 0
reset_cube.twist.linear.x = 0
reset_cube.twist.linear.y = 0
reset_cube.twist.linear.z = 0
set_model = SetModelState()
set_model.request.model_state = reset_cube
# rospy.wait_for_service("cube2", [0, 0, 0],[0, 0, 0], )
# '{model_state: { model_name: cube2, pose: { position: { x: 0, y: 0 ,z: 1 }, orientation: {x: 0, y: 0.491983115673, z: 0, w: 0.870604813099 } }, twist: { linear: {x: 0.0 , y: 0 ,z: 0 } , angular: { x: 0.0 , y: 0 , z: 0.0 } } , reference_frame: world } }'
rospy.wait_for_service('/gazebo/set_model_configuration')
try:
reset_joints("robot", "robot_description", ['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], [1.0,-1.8,0.0, 0.0, 0.0, 0.0])
except (rospy.ServiceException) as e:
print "reset_joints failed!"
rospy.wait_for_service('/gazebo/pause_physics')
try:
pause()
except (rospy.ServiceException) as e:
print "rospause failed!"
rospy.wait_for_service('/gazebo/unpause_physics')
try:
unpause()
except (rospy.ServiceException) as e:
print "/gazebo/pause_physics service call failed"
print "reset done."
def set_position(self, position):
model_state_resp = self.get_model_state(model_name="mobile_base")
model_state = SetModelState()
model_state.model_name = "mobile_base"
model_state.twist = Twist()
model_state.reference_frame = "world"
model_state.pose = model_state_resp.pose
model_state.pose.position.x = position[0]
model_state.pose.position.y = position[1]
self.set_model_state(model_state=model_state)
def position_indicator(self):
state = SetModelState()
state.model_name = "indicator"
pose = Pose()
pose.position.x = self.center[0]
pose.position.y = (-1) * self.center[1]
pose.position.z = (-1) * self.center[2]
state.pose = pose
state.twist = Twist()
state.reference_frame = ""
self.setModelState(state)
def teleport(self, location):
"""
Teleport robot's position.
location: string
location name
"""
if location == 0:
print("\teleporting to lobby")
else:
print("\teleporting to room {}".format(location))
location = self.numbered_locations[location]
# get location data
lx = self.locations[location]["lx"]
ly = self.locations[location]["ly"]
az = 0.0
if "az" in self.locations[location]:
az = self.locations[location]["az"]
# send location data to service
rospy.wait_for_service("/gazebo/set_model_state")
set_model_state = rospy.ServiceProxy("/gazebo/set_model_state",
SetModelState)
model_state = SetModelState()
model_state.model_name = "mobile_base"
pose = Pose()
pose.position.x = lx
pose.position.y = ly
orientation = quaternion_from_euler(0, 0, az)
pose.orientation.x = orientation[0]
pose.orientation.y = orientation[1]
pose.orientation.z = orientation[2]
pose.orientation.w = orientation[3]
model_state.pose = pose
twist = Twist()
twist.linear.x = 0.0
twist.angular.z = 0.0
model_state.twist = twist
model_state.reference_frame = "map"
set_model_state(model_state)
print("reached {}\n".format(location))
def set_position(self, position):
model_state_resp = self.get_model_state(model_name="piano2")
model_state = SetModelState()
model_state.model_name = "piano2"
model_state.pose = model_state_resp.pose
model_state.twist = Twist()
model_state.reference_frame = "world"
model_state.pose.position.x = position.X
model_state.pose.position.y = position.Y
model_state.pose.position.z = position.Z
self.set_model_state(model_state=model_state)
def set_steering_angle(self, quat):
model_state_resp = self.get_model_state(model_name="piano2")
model_state = SetModelState()
model_state.model_name = "piano2"
model_state.pose = model_state_resp.pose
model_state.twist = Twist()
model_state.reference_frame = "world"
model_state.pose.orientation.x = quat[0]
model_state.pose.orientation.y = quat[1]
model_state.pose.orientation.z = quat[2]
model_state.pose.orientation.w = quat[3]
self.set_model_state(model_state=model_state)
def set_heading_angle(self, heading_angle):
model_state_resp = self.get_model_state(model_name="mobile_base")
model_state = SetModelState()
model_state.model_name = "mobile_base"
model_state.pose = model_state_resp.pose
model_state.twist = Twist()
model_state.reference_frame = "world"
quat = tf.transformations.quaternion_from_euler(0, 0, heading_angle)
model_state.pose.orientation.x = quat[0]
model_state.pose.orientation.y = quat[1]
model_state.pose.orientation.z = quat[2]
model_state.pose.orientation.w = quat[3]
self.set_model_state(model_state=model_state)
def set_state(self, se3):
model_state_resp = self.get_model_state(model_name="piano2")
model_state = SetModelState()
model_state.model_name = "piano2"
model_state.pose = model_state_resp.pose
model_state.twist = Twist()
model_state.reference_frame = "world"
model_state.pose.position.x = se3.X
model_state.pose.position.y = se3.Y
model_state.pose.position.z = se3.Z
model_state.pose.orientation.x = se3.q[0]
model_state.pose.orientation.y = se3.q[1]
model_state.pose.orientation.z = se3.q[2]
model_state.pose.orientation.w = se3.q[3]
self.set_model_state(model_state=model_state)
def __init__(self):
#-------point cloud without color-------
rospy.loginfo("Start to set gazebo pose")
self.set_gazebo_srv = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState)
self.save_image_srv = rospy.ServiceProxy("/save_image", Trigger)
self.state = ModelState()
self.robot_state = ModelState()
self.set_model_state = SetModelState()
self.theta_range = (-90, 90)
self.distance_range = (3, 7.5)
self.yaw_range = (0, 360)
self.counter = 0
self.object_list = ['person_walking', 'person_walking_clone', 'person_walking_clone_0', 'person_walking_clone_1',\
'person_walking_clone_2', 'person_walking_clone_3', 'person_walking_clone_4',\
'person_standing', 'person_standing_clone', 'person_standing_clone_0', 'person_standing_clone_1'\
, 'person_standing_clone_2']
while(True):
self.set_gazebo_pose()
rospy.sleep(2.5)
self.save_image()
rospy.sleep(1.5)
def set_state(self, x, y, s, vx=0, vy=0, vs=0):
model_state_resp = self.get_model_state(model_name="ackermann_vehicle")
model_state = SetModelState()
model_state.model_name = "ackermann_vehicle"
model_state.pose = model_state_resp.pose
model_state.twist = Twist()
model_state.reference_frame = "world"
model_state.pose.position.x = x
model_state.pose.position.y = y
quat = tf.transformations.quaternion_from_euler(0,0,s)
model_state.pose.orientation.x = quat[0]
model_state.pose.orientation.y = quat[1]
model_state.pose.orientation.z = quat[2]
model_state.pose.orientation.w = quat[3]
model_state.twist.linear.x = vx
model_state.twist.linear.y = vy
model_state.twist.angular.z = vs
self.set_model_state(model_state=model_state)
def moveModel(model, position, r, p, y):
try:
service = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
twist = Twist()
pose = Pose()
pose.position = position
#rpy to quaternion:
quat = quaternion_from_euler(r, p, y)
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
req = SetModelState()
req.model_name = model
req.pose = pose
req.twist = twist
req.reference_frame = ""
resp = service(req)
except rospy.ServiceException, e:
print("Service call failed: %s" %e)
def _reset(self):
if self.proc is not None:
os.system("rosnode kill /rviz")
os.system("rosnode kill /move_base")
os.system("rosnode kill /pcl_filter")
rospy.sleep(5)
state = SetModelState()
state.model_name = 'mobile_base'
state.pose = self.start
state.twist = Twist()
state.reference_frame = ''
rospy.wait_for_service('/gazebo/set_model_state')
try:
self.set_model_state(state)
except (rospy.ServiceException) as e:
print("/gazebo/set_model_state service call failed")
reset = String()
reset.data = "r"
self.reset_pub.publish(reset)
rospy.sleep(3)
self.goal = Pose()
self.goal.position.x = random.random() * (
self.map_size_x_max - self.map_size_x_min) + self.map_size_x_min
self.goal.position.y = random.random() * (
self.map_size_y_max - self.map_size_y_min) + self.map_size_x_min
self.goal.orientation.w = 1
print(self.goal.position)
# Unpause simulation to make observation
rospy.wait_for_service('/gazebo/unpause_physics')
try:
#resp_pause = pause.call()
self.unpause()
except (rospy.ServiceException) as e:
print("/gazebo/unpause_physics service call failed")
#try to initialize SLAM
vel_cmd = Twist()
vel_cmd.linear.x = -0.4
r = rospy.Rate(10)
count = 0
status_ = Bool()
while status_.data is False and count < 10:
t = time.time()
if count % 5 is 0:
vel_cmd.linear.x = -vel_cmd.linear.x
#rospy.sleep(2)
while time.time() - t < 2.0:
self.vel_pub.publish(vel_cmd)
r.sleep()
status_ = rospy.wait_for_message('/ORB_SLAM2/Status',
Bool,
timeout=5)
print(status_)
count += 1
rospy.sleep(1)
# what will happen if SLAM is still not initialized
if status_ is False:
state = self._reset()
return state
rospy.sleep(2)
#self.port = os.environ["ROS_PORT_SIM"]
self.proc = subprocess.Popen([
"roslaunch", "-p", self.port,
"/home/rrc/gym-gazebo/gym_gazebo/envs/assets/launch/navigation_stack_rl.launch"
])
rospy.sleep(5)
goal_published = PoseStamped()
goal_published.pose = self.goal
goal_published.header.frame_id = "odom"
self.goal_pub.publish(goal_published)
rospy.sleep(1)
goal_done_ = self.goal_done
dist_ = self.dist
costmap_data_ = self.costmap_data
collision_ = self.collision
feature_ = None
while feature_ is None:
try:
feature_ = rospy.wait_for_message('/ORB_SLAM2/FeatureInfo',
Bool,
timeout=5)
except:
pass
# pause simulation
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")
temp1 = np.asarray(
[self.prev_cmd_vel.linear.x, self.prev_cmd_vel.angular.z, dist_])
temp2 = np.asarray(feature_.data)
temp3 = np.asarray(costmap_data_)
state = np.concatenate((temp1, temp2, temp3), axis=0)
return state
import rospy from geometry_msgs.msg import Point32, Twist, Pose from matplotlib import pyplot as plt from gp_abstract_sim.msg import path_points from nav_msgs.msg import Odometry from tf.transformations import euler_from_quaternion from gazebo_msgs.msg import ModelState from math import sqrt, pow, atan2 from std_msgs.msg import Float32 from gazebo_msgs.srv import GetModelState, SetModelState ########################################################################### speed = Twist() state_msg = ModelState() gestate = GetModelState() sestate = SetModelState() new_data = Odometry() current_x = 0.0 current_y = 0.0 current_yaw = 0.0 flagdamn = False flag = False gx = 0.0 gy = 0.0 gyaw = 0.0 currentPath = path_points() size = 0 delta_x = 0.0 delta_y = 0.0
def setModelState(modelStateObj):
return SetModelState(model_state=modelStateObj)
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()
