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