def set_model_state(name: str):
model_state = ModelState()
model_state.pose = Pose()
model_state.model_name = name
model_state.pose.position.x = (
self._config.ros_config.ros_launch_config.x_pos
)
if "fleeing" in name:
model_state.pose.position.x += (
self._config.ros_config.ros_launch_config.distance_tracking_fleeing_m
)
model_state.pose.position.y = (
self._config.ros_config.ros_launch_config.y_pos
)
model_state.pose.position.z = (
self._config.ros_config.ros_launch_config.z_pos
)
yaw = (
self._config.ros_config.ros_launch_config.yaw_or
if "fleeing" not in name
else self._config.ros_config.ros_launch_config.yaw_or + 3.14
)
(
model_state.pose.orientation.x,
model_state.pose.orientation.y,
model_state.pose.orientation.z,
model_state.pose.orientation.w,
) = quaternion_from_euler((0, 0, yaw))
self._set_model_state.wait_for_service()
self._set_model_state(model_state)
def set_model_state(name: str):
model_state = ModelState()
model_state.pose = Pose()
model_state.model_name = name
model_state.pose.position.x = 0
if 'fleeing' in name:
model_state.pose.position.x += 3. # fixed distance
model_state.pose.position.y = 0
model_state.pose.position.z = 0
yaw = 0 if 'fleeing' not in name else 3.14
model_state.pose.orientation.x, model_state.pose.orientation.y, model_state.pose.orientation.z, \
model_state.pose.orientation.w = quaternion_from_euler((0, 0, yaw))
self._set_model_state.wait_for_service()
self._set_model_state(model_state)
def set_model_state(name: str):
model_state = ModelState()
model_state.pose = Pose()
model_state.model_name = name
model_state.pose.position.x = self._config.ros_config.ros_launch_config.x_pos
if 'fleeing' in name:
model_state.pose.position.x += self._config.ros_config.ros_launch_config.distance_tracking_fleeing_m
model_state.pose.position.y = self._config.ros_config.ros_launch_config.y_pos
model_state.pose.position.z = self._config.ros_config.ros_launch_config.z_pos
yaw = self._config.ros_config.ros_launch_config.yaw_or if 'fleeing' not in name else \
self._config.ros_config.ros_launch_config.yaw_or + 3.14
model_state.pose.orientation.x, model_state.pose.orientation.y, model_state.pose.orientation.z, \
model_state.pose.orientation.w = quaternion_from_euler((0, 0, yaw))
self._set_model_state.wait_for_service()
self._set_model_state(model_state)