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