def set_robot_pos(self, x, y, theta):
# call service move_model
pose = Pose2D()
pose.x = x
pose.y = y
pose.theta = theta
srv_request = MoveModelRequest()
srv_request.name = self.ROBOT_NAME
srv_request.pose = pose
# call service
rospy.wait_for_service('%s/move_model' % self.NS)
self._service_client_move_robot_to(srv_request)
# publish robot position
self.pub_initial_position(x, y, theta)
def move_robot(self, pose: Pose2D):
"""move the robot to a given position
Args:
pose (Pose2D): target postion
"""
# call service move_model
srv_request = MoveModelRequest()
srv_request.name = self.ROBOT_NAME
srv_request.pose = pose
# call service
self._srv_move_model(srv_request)
if self.is_training_mode:
# a necessaray procedure to let the flatland publish the
# laser,odom's Transformation, which are needed for creating
# global path
for _ in range(self.LASER_UPDATE_RATE + 1):
self._step_world()
def move_obstacle(self, obstacle_name: str, x: float, y: float, theta: float):
"""move the obstacle to a given position
Args:
obstacle_name (str): [description]
x (float): [description]
y (float): [description]
theta (float): [description]
"""
assert obstacle_name in self.obstacle_name_list, "can't move the obstacle because it has not spawned in the flatland"
# call service move_model
srv_request = MoveModelRequest()
srv_request.name = obstacle_name
srv_request.pose.x = x
srv_request.pose.y = y
srv_request.pose.theta = theta
self._srv_move_model(srv_request)
def reset_pos_obstacles_random(self,
active_obstacle_rate: float = 1,
forbidden_zones: Union[list, None] = None):
"""randomly set the position of all the obstacles. In order to dynamically control the number of the obstacles within the
map while keep the efficiency. we can set the parameter active_obstacle_rate so that the obstacles non-active will moved to the
outside of the map
Args:
active_obstacle_rate (float): a parameter change the number of the obstacles within the map
forbidden_zones (list): a list of tuples with the format (x,y,r),where the the obstacles should not be reset.
"""
active_obstacle_names = random.sample(
self.obstacle_name_list,
int(len(self.obstacle_name_list) * active_obstacle_rate))
non_active_obstacle_names = set(
self.obstacle_name_list) - set(active_obstacle_names)
# non_active obstacles will be moved to outside of the map
resolution = self.map.info.resolution
pos_non_active_obstacle = Pose2D()
pos_non_active_obstacle.x = self.map.info.origin.position.x - \
resolution * self.map.info.width
pos_non_active_obstacle.y = self.map.info.origin.position.y - \
resolution * self.map.info.width
for obstacle_name in active_obstacle_names:
move_model_request = MoveModelRequest()
move_model_request.name = obstacle_name
# TODO 0.2 is the obstacle radius. it should be set automatically in future.
move_model_request.pose.x, move_model_request.pose.y, move_model_request.pose.theta = get_random_pos_on_map(
self._free_space_indices, self.map, 0.2, forbidden_zones)
self._srv_move_model(move_model_request)
for non_active_obstacle_name in non_active_obstacle_names:
move_model_request = MoveModelRequest()
move_model_request.name = non_active_obstacle_name
move_model_request.pose = pos_non_active_obstacle
self._srv_move_model(move_model_request)