def roll_tests():
states = [
SingleTest(
twist(0.1, 0, 0, 0.1, 0, 0),
pose(-2, 0, 0.7, 0, 0, 0),
goal_pose=pose(0, 0, 0.7, 90, 0, 0),
dp_mode=ControlModeEnum.POSITION_HEADING_HOLD,
),
]
return states
def __init__(self, surge_vel, sway_vel, initial_angle, end_pose=None):
SingleTest.__init__(self,
twist(surge_vel, sway_vel, 0, 0, 0, 0),
pose(-1.5, 0, 0.7, 0, 0, initial_angle),
timeout=20,
dp_mode=ControlModeEnum.ORIENTATION_DEPTH_HOLD,
end_pose=end_pose)
def __init__(self, yaw_vel):
SingleTest.__init__(
self,
twist(0.0, 0, 0, 0.0, 0, yaw_vel),
pose(0, 0, 0.7, 0, 0, 0),
timeout=15,
dp_mode=ControlModeEnum.DEPTH_HOLD,
)
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 __init__(self,
heave_vel,
surge_vel,
initial_angle,
initial_depth,
end_pose=None):
SingleTest.__init__(self,
twist(surge_vel, 0, heave_vel, 0, 0, 0),
pose(-1.5, 0, initial_depth, 0, 0,
initial_angle),
dp_mode=ControlModeEnum.ORIENTATION_HOLD,
end_pose=end_pose)
#!/usr/bin/python3
from math import pi
import rospy
from smach import StateMachine, Sequence
from smach_ros import IntrospectionServer
from std_msgs.msg import String
from geometry_msgs.msg import Pose
from helper import create_sequence, point, pose, ControlModeEnum
from common_states import GoToState
if __name__ == "__main__":
rospy.init_node("go_home_fsm")
state = GoToState(pose(0, 0, 0.7, 0, 0, 0))
state.execute(None)
def test_dp():
test_pose = pose(0, 0, 0.7, 0, 0, 0)
return DpState(test_pose)
def __init__(self, velocity, start_depth, end_pose=None):
SingleTest.__init__(self,
twist(0, 0, velocity, 0, 0, 0),
pose(0, 0, start_depth, 0, 0, 0),
dp_mode=ControlModeEnum.ORIENTATION_HOLD,
end_pose=end_pose)
def __init__(self, velocity, orientation, end_pose=None):
SingleTest.__init__(self,
twist(Y=velocity),
pose(-1.5, 0, 0.7, 0, 0, orientation),
dp_mode=ControlModeEnum.ORIENTATION_DEPTH_HOLD,
end_pose=end_pose)
HeaveSurgeTest(-0.05, 0.1, 0, 1.0),
HeaveSurgeTest(-0.05, 0.2, 0, 1.0),
HeaveSurgeTest(-0.1, 0.1, 0, 1.0),
HeaveSurgeTest(-0.1, 0.2, 0, 1.0),
HeaveSurgeTest(-0.15, 0.1, 0, 1.0),
HeaveSurgeTest(-0.15, 0.2, 0, 1.0, end_pose=HOME_POSE),
HeaveSurgeTest(0.05, -0.1, 180, 0.5),
HeaveSurgeTest(0.05, -0.2, 180, 0.5),
HeaveSurgeTest(0.10, -0.1, 180, 0.5),
HeaveSurgeTest(0.10, -0.2, 180, 0.5),
HeaveSurgeTest(0.15, -0.1, 180, 0.5),
HeaveSurgeTest(0.15, -0.2, 180, 0.5, end_pose=HOME_POSE),
HeaveSurgeTest(-0.05, -0.1, 180, 1.0),
HeaveSurgeTest(-0.05, -0.2, 180, 1.0),
HeaveSurgeTest(-0.10, -0.1, 180, 1.0),
HeaveSurgeTest(-0.10, -0.2, 180, 1.0),
HeaveSurgeTest(-0.15, -0.1, 180, 1.0),
HeaveSurgeTest(-0.15, -0.2, 180, 1.0),
]
return states
if __name__ == "__main__":
rospy.init_node("system_identification_sm")
HOME_POSE = pose(0, 0, 0.7, 0, 0, 0)
states = yaw_tests()
sm = create_sequence(states)
sm.execute()