def __init__(self, goal_pose):
"""State that moves to a goal pose by first using LOS guindace for traveling
potentially long distances and then DP guidance for fine tuned positioning.
Args:
goal_pose (geometry_msgs/Pose): Pose the drone will travel to.
"""
State.__init__(self, outcomes=["preempted", "succeeded", "aborted"])
self.goal_pose = goal_pose
self.los_state = LosState(self.goal_pose.position)
self.dp_state = DpState(self.goal_pose)
self.sm = create_sequence([self.los_state, self.dp_state],
state_names=["los_state", "dp_state"])
def visit_waypoints():
pose1 = Pose()
pose1.position.z = 1.0
pose1.position.x = 2
pose2 = Pose()
pose2.position.z = 0.5
sm = create_sequence(
[
GoToState(pose1),
GoToState(pose2),
]
)
introspection_server = IntrospectionServer(str(rospy.get_name()), sm, '/sm_root')
introspection_server.start()
sm.execute()
def __init__(self,
twist,
start_pose,
dp_mode=ControlModeEnum.OPEN_LOOP,
goal_pose=None,
timeout=20,
goal_boundry=[0.5, 0.5, 0.2, 0.15, 0.15, 0.15],
end_pose=None):
State.__init__(self, outcomes=["preempted", "succeeded", "aborted"])
x_min = -2
x_max = 2
y_min = -0.85
y_max = 0.85
z_min = 0.4
z_max = 1.05
states = [
GoToState(start_pose),
VelState(twist, dp_control_mode=dp_mode),
Monitor(
goal_pose=goal_pose,
max_duration=timeout,
pool_bounds=[
(x_min, x_max),
(y_min, y_max),
(z_min, z_max),
],
goal_boundry=goal_boundry,
odom_topic="/odometry/filtered",
)
]
if not end_pose:
states.append(GoToState(start_pose))
else:
states.append(GoToState(end_pose))
names = ["go_to_start", "set_velocity", "monitor", "back_to_start"]
self.sm = create_sequence(states, state_names=names)
state = VelState(twist)
res = state.execute(None)
rospy.loginfo(str(res))
def test_vel():
twist = Twist()
twist.linear.x = 0.25
twist.angular.x = 0.0
states = [
GoToState(pose(-1.5, 0, 0.7, 0, 0, 0)),
VelState(twist, dp_control_mode=ControlModeEnum.ORIENTATION_DEPTH_HOLD)
]
return states
def test_dp():
test_pose = pose(0, 0, 0.7, 0, 0, 0)
return DpState(test_pose)
def test_los():
goal_pos = Point()
goal_pos.z = 1
state = los_state(goal_pos)
state.execute(None)
if __name__ == "__main__":
rospy.init_node("pooltest_fsm")
states = test_vel()
sm = create_sequence(states)
sm.execute()