def _set_field(self, msg: Union[String, Twist, RosReward,
CommonCommonStateBatteryStateChanged,
Odometry], args: Tuple) -> None:
field_name, _ = args
if field_name == 'fsm_state':
self._fsm_state = FsmState[msg.data]
elif field_name == 'action':
self._action = Action(actor_name='applied_action',
value=process_twist(msg).value)
elif field_name == 'reward':
self._reward = msg.reward
self._terminal_state = TerminationType[msg.termination]
elif field_name == 'reference_pose':
self._reference_pose = np.asarray(
[msg.point.x, msg.point.y, msg.point.z])
self._calculate_ref_in_img()
elif field_name == 'battery':
self._battery = msg.percent
elif field_name == 'trajectory':
global_pose = process_odometry(msg)
self._trajectory.append(global_pose)
elif field_name == 'camera_orientation':
self._camera_orientation = float(msg.angular.y)
else:
raise NotImplementedError
def _set_field(self, msg: Union[String, Twist, RosReward,
CommonCommonStateBatteryStateChanged,
Odometry], args: Tuple) -> None:
field_name, _ = args
if field_name == 'fsm_state':
self._fsm_state = FsmState[msg.data]
elif field_name == 'action':
self._action = Action(actor_name='applied_action',
value=process_twist(msg).value)
elif field_name == 'reference_pose':
self._reference_pose = np.asarray(
[msg.point.x, msg.point.y, msg.point.z])
else:
raise NotImplementedError
def run(self):
rate = rospy.Rate(self._rate_fps)
while not rospy.is_shutdown():
if self._fsm_state == FsmState.TakenOver and self._motors_enabled:
for agent, publisher in self._publishers.items():
if agent in self._height.keys(
): # don't publish if we don't know the height yet
twist = self._get_twist(agent)
publisher.publish(twist)
self._control_norm_window.append(
process_twist(twist).norm())
if len(self._control_norm_window
) > self._control_norm_window_length:
self._control_norm_window.pop(0)
if len(self._control_norm_window) == self._control_norm_window_length \
and np.mean(self._control_norm_window) < 0.5:
cprint(f'reached stable height', self._logger)
self._go_publisher.publish(Empty())
rate.sleep()
def _set_field(self, msg: Union[Twist, Odometry, String],
args: tuple) -> None:
field_name, _ = args
# print(f'set field {field_name} with {msg}')
if field_name == 'fsm_state':
dump = False
if self._fsm_state == FsmState.Running and FsmState[
msg.data] == FsmState.TakenOver:
dump = True
if self._fsm_state != FsmState[msg.data]:
self._fsm_state = FsmState[msg.data]
print(f'set state to {self._fsm_state}')
if dump:
self._dump()
self._reset()
elif field_name == 'action':
self._last_action = [float(p) for p in process_twist(msg).value]
elif field_name == 'reference_pose':
self._last_reference_pose = [msg.point.x, msg.point.y, msg.point.z]
elif field_name == 'odometry':
self._last_odom = [float(p) for p in process_odometry(msg)]
def _set_field(self, msg: Union[String, Twist, RosReward,
CommonCommonStateBatteryStateChanged],
args: Tuple) -> None:
field_name, sensor_stats = args
if field_name == 'fsm_state':
self._fsm_state = FsmState[msg.data]
elif field_name == 'action':
self._action = Action(actor_name='applied_action',
value=process_twist(msg).value)
elif field_name == 'reward':
self._reward = msg.reward
self._terminal_state = TerminationType[msg.termination]
elif field_name == 'waypoint':
self._waypoint = np.asarray(msg.data)
elif field_name == 'battery':
self._battery = msg.percent
else:
raise NotImplementedError
cprint(f'set field {field_name}',
self._logger,
msg_type=MessageType.debug)
def _set_field(self, msg, args: Tuple) -> None:
field_name, sensor_stats = args
if field_name == "fsm_state":
self.fsm_state = FsmState[msg.data]
elif field_name == "observation":
msg_type = camelcase_to_snake_format(ros_message_to_type_str(msg))
self.observation = eval(f"process_{msg_type}(msg, sensor_stats)")
elif field_name == "action":
self.action = Action(
actor_name=self._config.ros_config.action_topic,
value=process_twist(msg).value,
)
elif field_name == "reward":
self.reward = msg.reward
self.terminal_state = TerminationType[msg.termination]
elif field_name.startswith("info:"):
info_key = field_name[5:]
msg_type = camelcase_to_snake_format(ros_message_to_type_str(msg))
self.info[info_key] = eval(f"process_{msg_type}(msg, sensor_stats)")
else:
raise NotImplementedError(f"{field_name}: {msg}")
cprint(f"set field {field_name}", self._logger, msg_type=MessageType.debug)
def get_action(self, sensor_data: dict = None) -> Action:
assert sensor_data is None
action = process_twist(self._update_twist())
action.actor_name = self._name
# cprint(f'action: {action}', self._logger, msg_type=MessageType.debug)
return action