def on_enter(self, userdata):
self.return_code = None
# Retrieve the relevant data
if self.given_action_topic == None:
# Not action topic define, check userdata
try :
self.current_action_topic = userdata.action_topic
except Exception as e:
Logger.logwarn('Failed to get relevant user data -\n%s' % (str(e)))
self.return_code = 'failed'
return
try:
if (self.client is None):
self.client = ProxyActionClient({self.current_action_topic: ClearOctomapAction},
self.wait_duration)
except Exception as e:
Logger.logwarn('Failed to set up the action client for %s\n%s' % (self.current_action_topic, str(e)))
self.return_code = 'failed'
return
try:
# Action Initialization
action_goal = ClearOctomapGoal()
self.client.send_goal(self.current_action_topic, action_goal)
if self.timeout_duration > rospy.Duration(0.0):
self.timeout_target = rospy.Time.now() + self.timeout_duration
else:
self.timeout_target = None
except Exception as e:
Logger.logwarn('Failed to send action goal for group - %s\n%s' % (self.name, str(e)))
self.return_code = 'failed'
def __init__(self):
smach.State.__init__(self,
outcomes=['Success', 'Failure'],
input_keys=['args'])
self.topic = "go_to_yaw"
self.client = ProxyActionClient(
{self.topic, riptide_controllers.msg.GoToYawAction})
def __init__(self,
controller='motion/controller/joint_state_head',
operational=True,
resources=[],
sync=True):
super(SetOperational, self).__init__(outcomes=['done', 'failure'])
# Store topic parameter for later use.
self._controller = controller
self._operational = operational
self._sync = sync
self._resources = resources
# create proxies
self._action_client = ProxyActionClient(
{self._controller: SetOperationalAction})
# check parameters
if not isinstance(resources, list) or not all(
[isinstance(r, str) for r in resources]):
raise ValueError(
'SetOperational: resource must be list of strings')
if not isinstance(operational, bool) or not isinstance(sync, bool):
raise ValueError(
'SetOperational: sync and operational parameters must be bool.'
)
# error in enter hook
self._error = False
def __init__(self,
move_group,
offset,
tool_link,
action_topic='/move_group'):
'''
Constructor
'''
super(MoveitToJointsDynState, self).__init__(
outcomes=['reached', 'planning_failed', 'control_failed'],
input_keys=['joint_values', 'joint_names'])
self._offset = offset
self._tool_link = tool_link
self._action_topic = action_topic
self._fk_srv_topic = '/compute_fk'
self._cartesian_srv_topic = '/compute_cartesian_path'
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._fk_srv = ProxyServiceCaller({self._fk_srv_topic: GetPositionFK})
self._cartesian_srv = ProxyServiceCaller(
{self._cartesian_srv_topic: GetCartesianPath})
self._current_group_name = move_group
self._joint_names = None
self._planning_failed = False
self._control_failed = False
self._success = False
self._traj_client = actionlib.SimpleActionClient(
'execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction)
self._traj_client.wait_for_server()
self._traj_exec_result = None
def __init__(self, config_name, move_group, action_topic = '/move_group', robot_name = ""):
'''
Constructor
'''
super(SrdfStateToMoveit, self).__init__(outcomes=['reached', 'planning_failed', 'control_failed', 'param_error'])
self._config_name = config_name
self._move_group = move_group
self._robot_name = robot_name
self._action_topic = action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._planning_failed = False
self._control_failed = False
self._success = False
self._srdf_param = None
if rospy.has_param("/robot_description_semantic"):
self._srdf_param = rospy.get_param("/robot_description_semantic")
else:
Logger.logerror('Unable to get parameter: /robot_description_semantic')
self._param_error = False
self._srdf = None
class BigTranslateParameterState(EventState):
"""
Changes the absolute position of the robot on the xy plane
-- topic string Topic to which the pose will be published.
-- x numeric The x value to translate
-- y numeric The y value to translate
># pose PoseStamped Pose to be published.
<= done Pose has been published.
"""
def __init__(self, topic, x, y):
"""Constructor"""
super(BigTranslateParameterState, self).__init__(outcomes=['Success', 'Failure'])
self._topic = topic
self._x = x
self._y = y
self.client = ProxyActionClient({self._topic: riptide_controllers.msg.MoveDistanceAction})
#self._pub = ProxyPublisher({self._topic: PoseStamped})
def execute(self, userdata):
if self.client.has_result(self._topic):
result = self.client.get_result(self._topic)
status = 'Success'
return status
def on_enter(self, userdata):
Logger.loginfo('Translating by vector <%f, %f>'%(self._x, self._y))
self.client.send_goal(self._topic, riptide_controllers.msg.MoveDistanceGoal(self._x, self._y))
class BigRollState(EventState):
"""
Publishes a pose from userdata so that it can be displayed in rviz.
-- topic string Topic to which the pose will be published.
># pose PoseStamped Pose to be published.
<= done Pose has been published.
"""
def __init__(self, topic):
"""Constructor"""
super(BigRollState, self).__init__(outcomes=['Success', 'Failure'],
input_keys=['angle'])
self._topic = topic
self.client = ProxyActionClient(
{self._topic: riptide_controllers.msg.GoToRollAction})
#self._pub = ProxyPublisher({self._topic: PoseStamped})
def execute(self, userdata):
if self.client.has_result(self._topic):
result = self.client.get_result(self._topic)
status = 'Success'
return status
def on_enter(self, userdata):
Logger.loginfo('Rolling with angle %f' % userdata.angle)
self.client.send_goal(
self._topic, riptide_controllers.msg.GoToRollGoal(userdata.angle))
def __init__(self, gripper_cmd, time=5.0):
"""Constructor"""
super(GripperCommandState, self).__init__(outcomes=['done', 'failed'])
self._joint_names = [
'gripper_finger_joint_l', 'gripper_finger_joint_r'
]
self._gripper_joint_positions = {
# gripper_finger_joint_l, gripper_finger_joint_r
0: [0.001, 0.001],
1: [0.010, 0.010] #NOTE: 0.011 results in goal tolerance violation
}
self._gripper_cmd = gripper_cmd
self._time = time
self._action_topic = "/arm_1/gripper_controller/follow_joint_trajectory"
self._client = ProxyActionClient(
{self._action_topic: FollowJointTrajectoryAction})
self._done = False
self._failed = False
def __init__(self):
smach.State.__init__(self,
outcomes=['Success', 'Failure'],
input_keys=['args'])
self.topic = "move_distance"
self.client = ProxyActionClient({
self.topic:riptide_controllers.msg.MoveDistanceAction})
def __init__(self, topic):
"""Constructor"""
super(BigRollState, self).__init__(outcomes=['Success', 'Failure'],
input_keys=['angle'])
self._topic = topic
self.client = ProxyActionClient(
{self._topic: riptide_controllers.msg.GoToRollAction})
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
self._action_topic = userdata.move_group_prefix + userdata.action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._move_group = userdata.move_group
self._joint_names = None
self._joint_names = userdata.joint_names
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(JointConstraint(joint_name=self._joint_names[i], position=userdata.joint_values[i]))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e)))
self._planning_failed = True
class Translate(EventState):
"""
Handles simple translations of the robot across the xy plane of the robot
oriented with the reference frame of it being upright.
@param x => float
the distance to move in the x direction
@param y => float
the distance to move in the y direction
"""
def __init__(self):
smach.State.__init__(self,
outcomes=['Success', 'Failure'],
input_keys=['args'])
self.topic = "move_distance"
self.client = ProxyActionClient({
self.topic:riptide_controllers.msg.MoveDistanceAction})
def on_enter(self, userdata):
Logger.loginfo('Translating by vector <%f, %f>'%(userdata.args['x'], userdata.args['y']))
self.client.send_goal(self.topic, riptide_controllers.msg.MoveDistanceGoal(userdata.args['x'], userdata.args['y']))
def execute(self, userdata):
if self.client.has_result(self.topic):
result = self.client.get_result(self.topic)
status = 'Success'
return status
class Align(EventState):
"""
Aligns robot to an object.
@param obj => string
the name of the object
@param bboxWidth => float
ratio of how big the bbox is to the camera input
@param hold => boolean
determines whether or not to stay in position
"""
def __init__(self):
smach.State.__init__(self,
outcomes=['Success', 'Failure'],
input_keys=['args'])
self.topic = "align"
self.client = ProxyActionClient(
{self.topic: riptide_controllers.msg.AlignAction})
def on_enter(self, userdata):
Logger.loginfo('Aligning robot')
self.client.send_goal(
self.topic,
riptide_controllers.msg.AlignGoal(userdata.args['obj'],
userdata.args['bboxWidth'],
userdata.args['hold']))
def execute(self, userdata):
if self.client.has_result(self.topic):
result = self.client.get_result(self.topic)
status = 'Success'
return status
class BigDistanceState(EventState):
"""
Publishes a pose from userdata so that it can be displayed in rviz.
-- topic string Topic to which the pose will be published.
># pose PoseStamped Pose to be published.
<= done Pose has been published.
"""
def __init__(self, topic):
"""Constructor"""
super(BigDistanceState, self).__init__(outcomes=['Success', 'Failure'],
input_keys=['object'], output_keys=['dist'])
self._topic = topic
self.client = ProxyActionClient({self._topic: riptide_controllers.msg.GetDistanceAction})
#self._pub = ProxyPublisher({self._topic: PoseStamped})
def execute(self, userdata):
if self.client.has_result(self._topic):
userdata.dist = self.client.get_result(self._topic).distance
status = 'Success'
return status
def on_enter(self, userdata):
Logger.loginfo('Moving %f m away from %s'%(2, userdata.object))
self.client.send_goal(self._topic, riptide_controllers.msg.GetDistanceGoal(userdata.object))
def __init__(self, move_group='whole_body', action_topic = '/move_group', orien_tolerance=True):
'''
Constructor
'''
super(hsr_MoveitToPoseGoalActionNew, self).__init__(outcomes=['reached', 'planning_failed', 'control_failed'], output_keys=['move_status'], input_keys=['pose_goal','plan_num'])
self._action_topic = action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self.orien_tolerance = orien_tolerance
self._move_group = move_group
self._planning_failed = False
self._control_failed = False
self._success = False
self.count_num = None
self._tolerance = 0.01
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander(self._move_group)
self.planning_frame = group.get_planning_frame()
self.planning_frame = '/map'
self.eef_link = group.get_end_effector_link()
group_names = robot.get_group_names()
scene = moveit_commander.PlanningSceneInterface()
def __init__(self, components=1023, timeout=5.0, wait_duration=2.0, action_topic=None):
'''
Constructor
'''
super(GetPlanningSceneState, self).__init__(
input_keys=['action_topic'],
outcomes=['done', 'failed'],
output_keys=['scene'] )
self.client = None
self.components = components
self.timeout_duration = rospy.Duration(timeout)
self.wait_duration = wait_duration
self.given_action_topic = action_topic
if (self.given_action_topic is not None) and (len(self.given_action_topic) > 0):
# If topic is defined, set the client up on startup
self.client = ProxyActionClient({self.given_action_topic: GetPlanningSceneAction},
self.wait_duration)
else:
self.given_action_topic = None
self.current_action_topic = self.given_action_topic
self.scene = list()
self.return_code = None
class Yaw(EventState):
"""
Handles rotating the robot's yaw by a given angle.
Yaw is like left/right movement.
@param angle => float
the angle to rotate by
"""
def __init__(self):
smach.State.__init__(self,
outcomes=['Success', 'Failure'],
input_keys=['args'])
self.topic = "go_to_yaw"
self.client = ProxyActionClient(
{self.topic, riptide_controllers.msg.GoToYawAction})
def on_enter(self, userdata):
Logger.loginfo('Yawing with angle %f' % userdata.args['angle'])
self.client.send_goal(
self.topic,
riptide_controllers.msg.GoToYawGoal(userdata.args['angle']))
def execute(self, userdata):
if self.client.has_result(self.topic):
result = self.client.get_result(self.topic)
status = 'Success'
return status
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
self._joint_config = userdata.joint_values
self._joint_names = userdata.joint_names
self._sub = ProxySubscriberCached({self._position_topic: JointState})
self._current_position = []
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
# Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
action_goal.request.allowed_planning_time = 1.0
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(
JointConstraint(joint_name=self._joint_names[i],
position=self._joint_config[i],
weight=1.0))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' %
(self._move_group, str(e)))
self._planning_failed = True
class Pitch(EventState):
"""
Handles rotating the robot's pitch.
Pitch is like tilting up/down.
@param angle => float
the angle to rotate by
"""
def __init__(self):
smach.State.__init__(self,
outcomes=['Success', 'Failure'],
input_keys=['args'])
self.topic = "go_to_pitch"
self.client = ProxyActionClient(
{self.topic: riptide_controllers.msg.GoToPitchAction})
#self.client.wait_for_server()
def on_enter(self, userdata):
self.client.send_goal(
self.topic,
riptide_controllers.msg.GoToPitchGoal(userdata.args['angle']))
Logger.loginfo('Pitching with angle %f' % userdata.args['angle'])
def execute(self, userdata):
if self.client.has_result(self.topic):
result = self.client.get_result(self.topic)
status = 'Success'
return status
def __init__(self, timeout=5.0, max_delay=-1.0, wait_duration=5, action_topic=None):
'''
Constructor
'''
super(ExecuteKnownTrajectoryState, self).__init__(
input_keys=['action_topic', 'trajectory'],
outcomes=['done', 'failed', 'param_error'],
output_keys=['status_text', 'goal_names', 'goal_values'] )
self.action_client = None
self.goal_names = None
self.goal_values = None
self.timeout_duration = rospy.Duration(timeout)
self.timeout_target = None
self.wait_duration = wait_duration
self.given_action_topic = action_topic
self.max_delay = None
if max_delay > 0.0:
self.max_delay = rospy.Duration(max_delay)
try:
if (self.given_action_topic is not None) and (len(self.given_action_topic) > 0):
# If topic is defined, set the client up on startup
self.action_client = ProxyActionClient({self.given_action_topic: ExecuteKnownTrajectoryAction},
self.wait_duration)
else:
self.given_action_topic = None # override ""
except Exception as e:
Logger.logerr(" %s - exception on initialization of ProxyMoveItClient \n%s"% (self.name, str(e)))
self.current_action_topic = self.given_action_topic
self.status_text = None
self.return_code = None
def __init__(self, direction):
'''
Constructor
'''
super(FootstepPlanRelativeState,
self).__init__(outcomes=['planned', 'failed'],
input_keys=['distance'],
output_keys=['plan_header'])
if not rospy.has_param("behavior/step_distance_forward"):
Logger.logerr(
"Need to specify parameter behavior/step_distance_forward at the parameter server"
)
return
if not rospy.has_param("behavior/step_distance_sideward"):
Logger.logerr(
"Need to specify parameter behavior/step_distance_sideward at the parameter server"
)
return
self._step_distance_forward = rospy.get_param(
"behavior/step_distance_forward")
self._step_distance_sideward = rospy.get_param(
"behavior/step_distance_sideward")
self._action_topic = '/vigir/footstep_manager/step_plan_request'
self._client = ProxyActionClient(
{self._action_topic: StepPlanRequestAction})
self._done = False
self._failed = False
self._direction = direction
class Depth(EventState):
"""
Handles moving the robot to a given depth in meters.
@param depth => float
the depth in meters the robot is aiming for
"""
def __init__(self):
smach.State.__init__(self,
outcomes=['Success', 'Failure'],
input_keys=['args'])
self.client = ProxyActionClient(
{"go_to_depth": riptide_controllers.msg.GoToDepthAction})
def on_enter(self, userdata):
Logger.loginfo('Moving to depth %f' % userdata.args['depth'])
#self.client.wait_for_server()
self.client.send_goal(
"go_to_depth",
riptide_controllers.msg.GoToDepthGoal(userdata.args['depth']))
def execute(self, userdata):
if self.client.has_result("go_to_depth"):
result = self.client.get_result("go_to_depth")
status = 'Success'
return status
def __init__(self, topic, x, y):
"""Constructor"""
super(BigTranslateParameterState, self).__init__(outcomes=['Success', 'Failure'])
self._topic = topic
self._x = x
self._y = y
self.client = ProxyActionClient({self._topic: riptide_controllers.msg.MoveDistanceAction})
def __init__(self, config_name='Home', move_group='arm', action_topic='/move_group', robot_name='m1n6s200',
position_topic='/m1n6s200_driver/joint_states', delta=1E-4):
'''
Constructor
'''
super(FeedbackSrdfStateToMoveit, self).__init__(outcomes=['reached', 'failed'])
self._position_topic = position_topic
self._delta = delta
self._config_name = config_name
self._move_group = move_group
self._robot_name = robot_name
self._action_topic = action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._planning_failed = False
self._control_failed = False
self._success = False
self._srdf_param = None
if rospy.has_param("/robot_description_semantic"):
self._srdf_param = rospy.get_param("/robot_description_semantic")
else:
Logger.logerror('Unable to get parameter: /robot_description_semantic')
self._param_error = False
self._srdf = None
def __init__(self, distance=1, ReplanPeriod=0.5):
"""Constructor"""
super(SaraFollow, self).__init__(outcomes=['failed'],
input_keys=['ID'])
# Prepare the action client for move_base
self._action_topic = "/move_base"
self._client = ProxyActionClient({self._action_topic: MoveBaseAction})
# Initialise the subscribers
self.entities_topic = "/entities"
self._pose_topic = "/robot_pose"
self._sub = ProxySubscriberCached({
self._pose_topic: Pose,
self.entities_topic: Entities
})
# Get the parameters
self.targetDistance = distance
self.ReplanPeriod = ReplanPeriod
# Initialise the status variables
self.MyPose = None
self.lastPlanTime = 0
def __init__(self,
controller='motion/controller/joint_trajectory',
outcomes=[
'success', 'partial_movement', 'invalid_pose', 'failure'
],
input_keys=[],
*args,
**kwargs):
# Declare outcomes and output keys
super(ExecuteJointTrajectoryBase, self).__init__(outcomes=outcomes,
input_keys=input_keys,
*args,
**kwargs)
# Connect to action server.
self._controller = controller
self._client = ProxyActionClient({
self._controller:
FollowJointTrajectoryAction
}) # pass required clients as dict (topic: type)
# It may happen that the action client fails to send the action goal, if _outcome is set then execute() exits immediately.
self._outcome = None
# log_once support
self._logged = False
self._logfunc = {
'info': Logger.loginfo,
'warn': Logger.logwarn,
'err': Logger.logerr,
'hint': Logger.loghint
}
class Distance(EventState):
"""
Finds the distance between the robot and a given object.
@param obj => string
the name of the object
"""
def __init__(self):
smach.State.__init__(self,
outcomes=['Success', 'Failure'],
input_keys=['args'],
output_keys=['type', 'args'])
self.topic = "get_distance"
self.client = ProxyActionClient(
{self.topic: riptide_controllers.msg.GetDistanceAction})
def on_enter(self, userdata):
Logger.loginfo('Moving %f m away from %s' % (2, userdata.args['obj']))
self.client.send_goal(
self.topic,
riptide_controllers.msg.GetDistanceGoal(userdata.args['obj']))
def execute(self, userdata):
if self.client.has_result(self.topic):
dist = client.get_result().distance
status = 'Success'
return status
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
self._config_name = userdata.config_name # Currently not used
self._robot_name = userdata.robot_name # Currently not used
self._move_group = userdata.move_group
self._action_topic = userdata.action_topic
self._joint_config = userdata.joint_values
self._joint_names = userdata.joint_names
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
# Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
action_goal.request.allowed_planning_time = 1.0
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(
JointConstraint(joint_name=self._joint_names[i],
position=self._joint_config[i],
weight=1.0))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
userdata.action_topic = self._action_topic # Save action topic to output key
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' %
(self._move_group, str(e)))
self._planning_failed = True
def __init__(self, topic):
"""Constructor"""
super(BigGateManeuverState,
self).__init__(outcomes=['Success', 'Failure'])
self._topic = topic
self.client = ProxyActionClient(
{self._topic: riptide_controllers.msg.GateManeuverAction})
def __init__(self, timeout=5.0, wait_duration=5, action_topic=None):
'''
Constructor
'''
super(ApplyPlanningSceneState, self).__init__(
input_keys=['action_topic'],
outcomes=['done', 'failed'])
self.client = None
self.timeout_duration = rospy.Duration(timeout)
self.wait_duration = wait_duration
self.action_topic = action_topic
if (self.action_topic is not None) and (len(self.action_topic) > 0):
# If topic is defined, set the client up on startup
self.client = ProxyActionClient({self.action_topic: ApplyPlanningSceneAction},
self.wait_duration)
else:
self.action_topic = None # override ""
self.moveit_client = None
try:
self.moveit_client = ProxyMoveItClient(None)
except Exception as e:
Logger.logerr(" %s - exception on initialization of ProxyMoveItClient \n%s"% (self.name, str(e)))
self.success = None
self.return_code = None
def __init__(self, config_name, move_group="", action_topic = '/move_group', robot_name=""):
'''
Constructor
'''
super(SrdfStateToMoveit, self).__init__(outcomes=['reached', 'planning_failed', 'control_failed', 'param_error'])
self._config_name = config_name
self._move_group = move_group
self._robot_name = robot_name
self._action_topic = action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._planning_failed = False
self._control_failed = False
self._success = False
self._srdf_param = None
if rospy.has_param("/robot_description_semantic"):
self._srdf_param = rospy.get_param("/robot_description_semantic")
else:
Logger.logerror('Unable to get parameter: /robot_description_semantic')
self._param_error = False
self._srdf = None
def __init__(self, controllers = ["left_arm_traj_controller", "right_arm_traj_controller"]):
'''Constructor'''
super(ExecuteTrajectoryBothArmsState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['trajectories'])
self._controllers = controllers
self._controllers = ["left_arm_traj_controller", "right_arm_traj_controller"] if not(controllers) else controllers
self._client = ProxyActionClient()
self._client_topics = dict()
self._active_topics = list()
for controller in self._controllers:
if "left_arm" in controller:
action_topic_left = "/trajectory_controllers/" + controller + "/follow_joint_trajectory"
self._client.setupClient(action_topic_left, FollowJointTrajectoryAction)
self._client_topics['left_arm'] = action_topic_left
elif "right_arm" in controller:
action_topic_right = "/trajectory_controllers/" + controller + "/follow_joint_trajectory"
self._client.setupClient(action_topic_right, FollowJointTrajectoryAction)
self._client_topics['right_arm'] = action_topic_right
else:
Logger.logwarn('The controller is neither a left nor a right arm trajectory controller!? %s' % str(controller))
self._failed = False
def __init__(self, desired_tilt):
'''Constructor'''
super(TiltHeadState, self).__init__(outcomes=['done', 'failed'])
if not rospy.has_param("behavior/joint_controllers_name"):
Logger.logerr("Need to specify parameter behavior/joint_controllers_name at the parameter server")
return
controller_namespace = rospy.get_param("behavior/joint_controllers_name")
# self._configs['flor']['same'] = {
# 20: {'joint_name': 'neck_ry', 'joint_values': [+0.00], 'controller': 'neck_traj_controller'}, # max -40 degrees
# 21: {'joint_name': 'neck_ry', 'joint_values': [-40.0], 'controller': 'neck_traj_controller'},
# 22: {'joint_name': 'neck_ry', 'joint_values': [-20.0], 'controller': 'neck_traj_controller'},
# 23: {'joint_name': 'neck_ry', 'joint_values': [+20.0], 'controller': 'neck_traj_controller'},
# 24: {'joint_name': 'neck_ry', 'joint_values': [+40.0], 'controller': 'neck_traj_controller'},
# 25: {'joint_name': 'neck_ry', 'joint_values': [+60.0], 'controller': 'neck_traj_controller'} # max +60 degrees
# }
self._action_topic = "/" + controller_namespace + \
"/neck_traj_controller" + "/follow_joint_trajectory"
self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction})
# Convert desired position to radians
self._desired_tilt = math.radians(desired_tilt)
self._failed = False
def __init__(self):
super(SpeechOutputState, self).__init__(outcomes=["done", "failed"], input_keys=["text"])
self._topic = "/speak"
self._client = ProxyActionClient({self._topic: maryttsAction})
self._error = False
def __init__(self, direction):
'''
Constructor
'''
super(FootstepPlanRelativeState, self).__init__(outcomes=['planned', 'failed'],
input_keys=['distance'],
output_keys=['plan_header'])
if not rospy.has_param("behavior/step_distance_forward"):
Logger.logerr("Need to specify parameter behavior/step_distance_forward at the parameter server")
return
if not rospy.has_param("behavior/step_distance_sideward"):
Logger.logerr("Need to specify parameter behavior/step_distance_sideward at the parameter server")
return
self._step_distance_forward = rospy.get_param("behavior/step_distance_forward")
self._step_distance_sideward = rospy.get_param("behavior/step_distance_sideward")
self._action_topic = '/vigir/footstep_manager/step_plan_request'
self._client = ProxyActionClient({self._action_topic: StepPlanRequestAction})
self._done = False
self._failed = False
self._direction = direction
def __init__(self):
super(SpeechOutputState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['text'])
self._topic = '/speak'
self._client = ProxyActionClient({self._topic: maryttsAction})
self._error = False
def __init__(self):
super(StartExploration, self).__init__(outcomes = ['succeeded', 'failed'])
self._action_topic = '/move_base'
self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction})
self._succeeded = False
self._failed = False
def __init__(self):
# See example_state.py for basic explanations.
super(MoveCameraState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['pan', 'tilt'])
self._topic = 'SetPTUState'
self._client = ProxyActionClient({self._topic: PtuGotoAction})
self._error = False
def __init__(self, sweep_type):
super(MetricSweepState, self).__init__(outcomes = ['sweeped', 'failed'])
self._sweep_type = sweep_type
self._topic = '/do_sweep'
self._client = ProxyActionClient({self._topic: SweepAction})
self._error = False
def __init__(self):
super(Explore, self).__init__(outcomes = ['succeeded', 'failed'], input_keys =['speed'])
self._action_topic = '/move_base'
self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction})
self._dynrec = Client("/vehicle_controller", timeout = 10)
self._defaultspeed = 0.1
self._succeeded = False
self._failed = False
def __init__(self):
super(FootstepPlanRealignCenterState, self).__init__(outcomes=['planned','failed'],
output_keys=['plan_header'])
self._action_topic = '/vigir/footstep_manager/step_plan_request'
self._client = ProxyActionClient({self._action_topic: StepPlanRequestAction})
self._failed = False
self._done = False
def __init__(self, controller):
'''
Constructor
'''
super(ExecuteTrajectoryState, self).__init__(outcomes=['done', 'failed'],
input_keys=['joint_trajectory'])
self._action_topic = controller + "/follow_joint_trajectory"
self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction})
self._failed = False
def __init__(self):
'''
Constructor
'''
super(ExecuteStepPlanActionState , self).__init__(outcomes=['finished','failed'],
input_keys=['plan_header'])
self._action_topic = "/vigir/footstep_manager/execute_step_plan"
self._client = ProxyActionClient({self._action_topic: ExecuteStepPlanAction})
self._failed = False
def __init__(self):
"""
Constructor
"""
super(LookAtWaypoint, self).__init__(outcomes=["reached", "failed"], input_keys=["waypoint"])
self._action_topic = "/pan_tilt_sensor_head_joint_control/look_at"
self._client = ProxyActionClient({self._action_topic: hector_perception_msgs.msg.LookAtAction})
self._failed = False
self._reached = False
class MetricSweepState(EventState):
'''
Robot performs a metric sweep at its current location.
-- sweep_type string Type of the sweep to do (select one of the provided options).
<= sweeped Sweep has been completed.
<= failed There has been an error when sweeping.
'''
COMPLETE = 'complete'
MEDIUM = 'medium'
SHORT = 'short'
SHORTEST = 'shortest'
def __init__(self, sweep_type):
super(MetricSweepState, self).__init__(outcomes = ['sweeped', 'failed'])
self._sweep_type = sweep_type
self._topic = '/do_sweep'
self._client = ProxyActionClient({self._topic: SweepAction})
self._error = False
def execute(self, userdata):
if self._error:
return 'failed'
if self._client.has_result(self._topic):
result = self._client.get_result(self._topic)
if result.success:
return 'sweeped'
else:
return 'failed'
def on_enter(self, userdata):
goal = SweepGoal()
goal.type = self._sweep_type
self._error = False
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
Logger.logwarn('Failed to send the Sweep command:\n%s' % str(e))
self._error = True
def on_exit(self, userdata):
if not self._client.has_result(self._topic):
self._client.cancel(self._topic)
Logger.loginfo('Cancelled active action goal.')
def __init__(self):
'''
Constructor
'''
super(MoveToWaypointState, self).__init__(outcomes=['reached', 'failed'],
input_keys=['waypoint'])
self._action_topic = '/move_base'
self._client = ProxyActionClient({self._action_topic: MoveBaseAction})
self._failed = False
self._reached = False
def __init__(self, action, duration = 1.0):
'''
Constructor
'''
super(GripperState, self).__init__(outcomes=['done', 'failed'])
self._action_topic = "/gripper_control/gripper_grasp_traj_controller/follow_joint_trajectory"
self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction})
self._failed = False
self._duration = duration
self._target_joint_value = action * 1.89
def __init__(self):
"""Constructor"""
super(MoveBaseState, self).__init__(outcomes = ['arrived', 'failed'],
input_keys = ['waypoint'])
self._action_topic = "/move_base"
self._client = ProxyActionClient({self._action_topic: MoveBaseAction})
self._arrived = False
self._failed = False
def __init__(self, max_attempts=1):
"""
Constructor
"""
super(PipeDetectionState, self).__init__(outcomes=["found", "unknown"])
self._action_topic = "/hector_five_pipes_detection_node/detect"
self._client = ProxyActionClient({self._action_topic: DetectObjectAction})
self._max_attempts = max_attempts
self._success = False
self._failed = False
def __init__(self, vel_scaling = 0.1):
"""Constructor"""
super(MoveitStartingPointState, self).__init__(outcomes=['reached', 'failed'],
input_keys=['trajectories'])
self._action_topic = "/vigir_move_group"
self._client = ProxyActionClient({self._action_topic: MoveAction})
self._vel_scaling = vel_scaling
self._failed = False
def __init__(self, rotation, duration=1.0):
"""
Constructor
"""
super(GripperRollState, self).__init__(outcomes=["done", "failed"])
self._action_topic = "/gripper_control/gripper_roll_traj_controller/follow_joint_trajectory"
self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction})
self._failed = False
self._duration = duration
self._target_joint_value = rotation
def __init__(self):
'''
Constructor
'''
super(LookAtPattern, self).__init__(outcomes=['succeeded', 'failed'],
input_keys=['pattern'])
self._action_topic = '/pan_tilt_sensor_head_joint_control/look_at'
self._client = ProxyActionClient({self._action_topic: hector_perception_msgs.msg.LookAtAction})
self._failed = False
self._succeeded = False
def __init__(self):
'''
Constructor
'''
super(MoveToWaypointState, self).__init__(outcomes=['reached', 'failed', 'update'],
input_keys=['waypoint','victim','speed'])
self._action_topic = '/move_base'
self._move_client = ProxyActionClient({self._action_topic: MoveBaseAction})
self.set_tolerance = rospy.ServiceProxy('/controller/set_alternative_tolerances', SetAlternativeTolerance)
self._failed = False
self._reached = False
def __init__(self, vel_scaling=0.1):
"""Constructor"""
super(MoveItMoveGroupPlanState, self).__init__(outcomes=['done', 'failed'],
input_keys=['desired_goal'],
output_keys=['plan_to_goal'])
self._action_topic = "/vigir_move_group"
self._client = ProxyActionClient({self._action_topic: MoveAction})
self._vel_scaling = vel_scaling
self._failed = False
self._done = False
def __init__(self, arm_side, target_config, time=2.0):
"""Constructor"""
super(GotoSingleArmJointConfigState, self).__init__(outcomes=["done", "failed"], input_keys=["current_config"])
if not rospy.has_param("behavior/robot_namespace"):
Logger.logerr("Need to specify parameter behavior/robot_namespace at the parameter server")
return
self._robot = rospy.get_param("behavior/robot_namespace")
if not rospy.has_param("behavior/joint_controllers_name"):
Logger.logerr("Need to specify parameter behavior/joint_controllers_name at the parameter server")
return
controller_namespace = rospy.get_param("behavior/joint_controllers_name")
################################ ATLAS ################################
self._configs = dict()
self._configs["flor"] = dict()
self._configs["flor"]["left"] = {
11: {"joint_name": "l_arm_wry2", "joint_value": -2.5},
12: {"joint_name": "l_arm_wry2", "joint_value": +2.5},
}
self._configs["flor"]["right"] = {
11: {"joint_name": "r_arm_wry2", "joint_value": +2.5},
12: {"joint_name": "r_arm_wry2", "joint_value": -2.5},
}
################################ THOR #################################
self._configs["thor_mang"] = dict()
self._configs["thor_mang"]["left"] = {
11: {"joint_name": "l_wrist_yaw2", "joint_value": 3.84},
12: {"joint_name": "l_wrist_yaw2", "joint_value": -3.84},
}
self._configs["thor_mang"]["right"] = {
11: {"joint_name": "r_wrist_yaw2", "joint_value": -3.84},
12: {"joint_name": "r_wrist_yaw2", "joint_value": 3.84},
}
#######################################################################
self._joint_name = self._configs[self._robot][arm_side][target_config]["joint_name"]
self._joint_value = self._configs[self._robot][arm_side][target_config]["joint_value"]
self._time = time
self._action_topic = (
"/" + controller_namespace + "/" + arm_side + "_arm_traj_controller" + "/follow_joint_trajectory"
)
self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction})
self._failed = False
def __init__(self):
# See example_state.py for basic explanations.
super(TweetPictureState, self).__init__(outcomes = ['picture_tweeted', 'tweeting_failed','command_error'], input_keys = ['picture_path', 'tweet_text'])
# Create the action client when building the behavior.
# This will cause the behavior to wait for the client before starting execution
# and will trigger a timeout error if it is not available.
# Using the proxy client provides asynchronous access to the result and status
# and makes sure only one client is used, no matter how often this state is used in a behavior.
self._topic = 'strands_tweets'
self._client = ProxyActionClient({self._topic: SendTweetAction}) # pass required clients as dict (topic: type)
# It may happen that the action client fails to send the action goal.
self._error = False
def __init__(self):
'''
Constructor
'''
super(MoveArmState, self).__init__(outcomes=['reached', 'planning_failed', 'control_failed'],
input_keys=['joint_config', 'group_name'])
self._action_topic = '/move_group'
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._joint_names = ['arm_joint_0', 'arm_joint_1', 'arm_joint_2', 'arm_joint_3']
self._planning_failed = False
self._control_failed = False
self._success = False
def __init__(self, pose_is_pelvis = False):
'''Constructor'''
super(CreateStepGoalState, self).__init__(outcomes = ['done', 'failed'],
input_keys = ['target_pose'],
output_keys = ['step_goal'])
self._action_topic = "/vigir/footstep_manager/generate_feet_pose"
self._client = ProxyActionClient({self._action_topic: GenerateFeetPoseAction})
self._pose_is_pelvis = pose_is_pelvis
self._done = False
self._failed = False
def __init__(self, motion_key, time_factor=1):
"""
Constructor
"""
super(MotionServiceState, self).__init__(outcomes=['done', 'failed'])
self.motion_key = motion_key
self.time_factor = time_factor
self._finish_time = None
self._motion_goal_ns = '/motion_service/motion_goal'
self._client = ProxyActionClient({self._motion_goal_ns: ExecuteMotionAction})
self._failed = False
self._done = False
def __init__(self, request, message):
'''
Constructor
'''
super(InputState, self).__init__(outcomes=['received', 'aborted', 'no_connection', 'data_error'],
output_keys=['data'])
self._action_topic = '/flexbe/behavior_input'
self._client = ProxyActionClient({self._action_topic: BehaviorInputAction})
self._request = request
self._message = message
self._connected = True
self._received = False
