Smatch 最基础的应用

Smatch 最基础的应用

remapping 参数

sm_top = smach.StateMachine(outcomes=['outcome4','outcome5'],
                          input_keys=['sm_input'],
                          output_keys=['sm_output'])
  with sm_top:
     smach.StateMachine.add('FOO', Foo(),
     						# transimation 把输出和下一个状态连接
                            transitions={'outcome1':'BAR',
                                         'outcome2':'outcome4'},
                            # remapping 把输入输出和状态机的输入输出连接起来：把状态机的输入，映射到状态的输入
                            remapping={'foo_input':'sm_input',
                                       'foo_output':'sm_data'})
     smach.StateMachine.add('BAR', Bar(),
                            transitions={'outcome2':'FOO'},
                            remapping={'bar_input':'sm_data',
                                       'bar_output1':'sm_output'})

示例 ： state machine 加入了 状态机的输入输出

#!/usr/bin/env python
import rospy
import smach
import smach_ros
import time
class Pick(smach.State):
    def __init__(self):
        smach.State.__init__(self,outcomes=['Y','N'],
                                input_keys=['pick_in'],
                                output_keys=['pick_out'])
     

    def execute(self,userdata):
        rospy.loginfo('Executing state Pick')
        # time.sleep(4)
        if userdata.pick_in<3:
            userdata.pick_out=userdata.pick_in+1
            return 'N'
        else: 
            return 'Y'
class Bar(smach.State):
    def __init__(self):
        smach.State.__init__(self,outcomes=['Y'],input_keys=['bar_in'])
    def execute(self,userdata):
        rospy.loginfo('Executing state BAr')
        rospy.loginfo('Counter=%f'%userdata.bar_in)
        time.sleep(1)
        return 'Y'


rospy.init_node('smach_example_state_machine')

sm=smach.StateMachine(outcomes=['stop'])
sm.userdata.sm_counter=0
with sm:
    
    smach.StateMachine.add('Pick',Pick(),
                            transitions={'N':'Bar','Y':'stop'},
                            remapping={'pick_in':'sm_counter',
                                        'pick_out':'sm_counter'})
    smach.StateMachine.add('Bar',Bar(),transitions={'Y':'Pick'},remapping={'bar_in':'sm_counter'})

outcome=sm.execute()
print(outcome)
rospy.loginfo('Counter=%f'%sm.userdata.sm_counter)

只用moveit控制机械臂抓取

#!/usr/bin/env python

import rospy
from math import pow, atan2, sqrt
from tf.transformations import *

import smach
import smach_ros
from smach_ros import SimpleActionState
from smach_ros import ServiceState

import threading
import time
# Navigation
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist

# Manipulator 
from geometry_msgs.msg import Pose
from open_manipulator_msgs.msg import JointPosition
from open_manipulator_msgs.msg import KinematicsPose
from open_manipulator_msgs.srv import SetJointPosition
from open_manipulator_msgs.srv import SetKinematicsPose

# AR Markers
from ar_track_alvar_msgs.msg import AlvarMarker
from ar_track_alvar_msgs.msg import AlvarMarkers

class getPoseOfTheObject(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded', 'aborted'],
                                    output_keys=['output_object_pose'])

        self.namespace = "om_with_tb3"
        self.marker_pose_sub = rospy.Subscriber(self.namespace + '/ar_pose_marker', AlvarMarkers, self.arMarkerMsgCallback)

        self.OFFSET_FOR_GOAL_HEIGHT = 0.150

    def arMarkerMsgCallback(self, ar_marker_pose_msg):
        if len(ar_marker_pose_msg.markers) == 0:
            self.ar_marker_pose = False
        else:            
            self.ar_marker_pose = AlvarMarker()
            self.ar_marker_pose = ar_marker_pose_msg.markers[0]

    def execute(self, userdata):
        if self.ar_marker_pose == False:
            rospy.logwarn('Failed to get pose of the marker')
            return 'aborted'
        else:
            object_pose = Pose()
            object_pose.position = self.ar_marker_pose.pose.pose.position
 
            object_pose.position.x += 0.0
            object_pose.position.y  = 0.0
            object_pose.position.z += self.OFFSET_FOR_GOAL_HEIGHT

            dist = math.sqrt((self.ar_marker_pose.pose.pose.position.x * self.ar_marker_pose.pose.pose.position.x) +
                        (self.ar_marker_pose.pose.pose.position.y * self.ar_marker_pose.pose.pose.position.y))

            if self.ar_marker_pose.pose.pose.position.y > 0:
                yaw = math.acos(self.ar_marker_pose.pose.pose.position.x / dist)
            else:
                yaw = (-1) * math.acos(self.ar_marker_pose.pose.pose.position.x / dist)

            roll = 0.0
            pitch = 0.0

            cy = math.cos(yaw * 0.5)
            sy = math.sin(yaw * 0.5)
            cr = math.cos(roll * 0.5)
            sr = math.sin(roll * 0.5)
            cp = math.cos(pitch * 0.5)
            sp = math.sin(pitch * 0.5)

            object_pose.orientation.w = cy * cr * cp + sy * sr * sp
            object_pose.orientation.x = cy * sr * cp - sy * cr * sp
            object_pose.orientation.y = cy * cr * sp + sy * sr * cp
            object_pose.orientation.z = sy * cr * cp - cy * sr * sp

            userdata.output_object_pose = object_pose
            rospy.loginfo('Succeeded to get pose of the object')
            return 'succeeded'

        self.marker_pose_sub

class getPoseOfTheBox(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded', 'aborted'],
                                    output_keys=['output_object_pose'])

        self.namespace = "om_with_tb3"
        self.marker_pose_sub = rospy.Subscriber(self.namespace + '/ar_pose_marker', AlvarMarkers, self.arMarkerMsgCallback