def _init_global_localization(self):
"""
Initialize global localization for amcl
"""
service_name = '/global_localization'
service_class = Empty
utils.call_service(service_name, service_class)
def unpause_sim(self):
"""
Resume the phsics update of gazebo
"""
service_name = '/gazebo/unpause_physics'
service_class = Empty
utils.call_service(service_name, service_class)
def pause_sim(self):
"""
Pause the physics updates of gazebo
"""
service_name = '/gazebo/pause_physics'
service_class = Empty
utils.call_service(service_name, service_class)
def reset_sim(self):
"""
Using _reset_type variable's value the corresponding reset service
of gazebo is called
"""
rospy.logdebug('GazeboConnection.reset_sim() start')
if self._reset_type == 'SIMULATION':
# reset the entire simulation including the time
service_name = '/gazebo/reset_simulation'
service_class = Empty
elif self._reset_type == 'WORLD':
# reset the model's poses
service_name = '/gazebo/reset_world'
service_class = Empty
else:
# do nothing
return
utils.call_service(service_name, service_class)
def _check_map_data_is_ready(self):
"""
Checks map service is operational
"""
rospy.logdebug(
'TurtleBot3LocalizeEnv._check_map_data_is_ready() start')
service_name = '/static_map'
service_class = GetMap
msg, _ = utils.call_service(service_name, service_class)
if msg.map.header.frame_id != self._global_frame_id:
rospy.logwarn('received map must be in the global frame')
self._map_data = self.__process_map_msg(msg.map)
# every time map is received perfrom following
self._publish_rnd_init_pose()
self._init_amcl(is_global=True)
def set_model_state(self, model_state):
"""
Sets the model state in gazebo through service call
:param gazebo_msgs.msg._ModelState.ModelState model_state: gazebo model pose and twist
"""
service_name = '/gazebo/set_model_state'
service_class = SetModelState
service_req = SetModelStateRequest()
service_req.model_state = model_state
response, is_successful = utils.call_service(service_name,
service_class,
service_req)
if is_successful and response.success:
pass # do nothing
else:
rospy.logwarn(response.status_message)
def spawn_sdf_model(self,
model_name: str,
initial_pose,
robot_namespace: str = '',
reference_frame: int = 'world'):
"""
Spawns a model (*.sdf) to gazebo through service call
:param str model_name: name of gazebo model to be spawn
geometry_msgs.msg._Pose.Pose initial_pose: initial pose of model
str robot_namespace: spawn model under this namespace
str reference_frame: initial_pose is defined relative to the frame of this model,
default is gazebo world frame
"""
if model_name in self.__current_models:
rospy.logwarn(
'model: %s already exists in gazebo so will be respawned',
model_name)
self.delete_model(model_name)
model_path = rospkg.RosPack().get_path('indoor_layouts') + '/models/'
file_xml = open(model_path + model_name + '/model.sdf')
# this should be an urdf or gazebo xml
model_xml = file_xml.read().replace('\n', '')
service_name = '/gazebo/spawn_sdf_model'
service_class = SpawnModel
service_req = SpawnModelRequest()
service_req.model_name = model_name
service_req.model_xml = model_xml
service_req.initial_pose = initial_pose
service_req.robot_namespace = robot_namespace
service_req.reference_frame = reference_frame
response, is_successful = utils.call_service(service_name,
service_class,
service_req)
if is_successful and response.success:
# add model from tracking list
self.__current_models.append(model_name)
else:
rospy.logwarn(response.status_message)
def spawn_urdf_model(self,
model_name: str,
initial_pose,
robot_namespace: str = '',
reference_frame: str = 'world'):
"""
Spawns a model (*.urdf) to gazebo through service call
:param str model_name: name of gazebo model to be spawn
geometry_msgs.msg._Pose.Pose initial_pose: initial pose of model
str robot_namespace: spawn model under this namespace
str reference_frame: initial_pose is defined relative to the frame of this model,
default is gazebo world frame
"""
model_path = rospkg.RosPack().get_path(
'turtlebot3_description') + '/urdf/'
p = os.popen("rosrun xacro xacro " + model_path + model_name +
'_waffle.urdf.xacro')
# this should be an urdf or gazebo xml
model_xml = p.read().replace('\n', '')
p.close()
service_name = '/gazebo/spawn_urdf_model'
service_class = SpawnModel
service_req = SpawnModelRequest()
service_req.model_name = model_name
service_req.model_xml = model_xml
service_req.initial_pose = initial_pose
service_req.robot_namespace = robot_namespace
service_req.reference_frame = reference_frame
response, is_successful = utils.call_service(service_name,
service_class,
service_req)
if is_successful and response.success:
# add model from tracking list
self.__current_models.append(model_name)
else:
rospy.logwarn(response.status_message)
def get_model_state(self,
model_name: str,
relative_entity_name: str = 'world'):
"""
Gets the model state from gazebo through service call
:param str model_name: name of gazebo model
str relative_entity_name: return pose and twist relative to this entity,
default is gazebo world frame
:return response received from service call
"""
service_name = '/gazebo/get_model_state'
service_class = GetModelState
service_req = GetModelStateRequest()
service_req.model_name = model_name
service_req.relative_entity_name = relative_entity_name
response, is_successful = utils.call_service(service_name,
service_class,
service_req)
return response
def delete_model(self, model_name: str):
"""
Delete a model from gazebo through service call
:param str model_name: name of gazebo model to be deleted
"""
service_name = '/gazebo/delete_model'
service_class = DeleteModel
service_req = DeleteModelRequest()
service_req.model_name = model_name
response, is_successful = utils.call_service(service_name,
service_class,
service_req)
if is_successful and response.success:
# remove model from tracking list
if model_name in self.__current_models:
self.__current_models.remove(model_name)
else:
rospy.logwarn(response.status_message)
time.sleep(0.2) # wait for some time